From 6c41645d77c75a56b33fcb2d6a1e7efdbe767c91 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Fri, 22 May 2026 21:25:10 -0700 Subject: [PATCH 1/7] 5-intaking-functional i made the intake pivot deploy --- .../frc/robot/commands/states/Intaking.java | 38 +++++++++++++++++++ .../java/frc/robot/constants/ConstMotion.java | 6 +++ 2 files changed, 44 insertions(+) create mode 100644 src/main/java/frc/robot/commands/states/Intaking.java diff --git a/src/main/java/frc/robot/commands/states/Intaking.java b/src/main/java/frc/robot/commands/states/Intaking.java new file mode 100644 index 0000000..463f5e8 --- /dev/null +++ b/src/main/java/frc/robot/commands/states/Intaking.java @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.constants.ConstMotion; +import frc.robot.subsystems.Motion; + +public class Intaking extends Command { + /** Creates a new Intaking. */ + public Intaking() { + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_DEPLOY); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index c01f0b1..be2cce3 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -4,11 +4,17 @@ package frc.robot.constants; +import static edu.wpi.first.units.Units.Degrees; + import com.ctre.phoenix6.configs.TalonFXConfiguration; +import edu.wpi.first.units.measure.Angle; + /** Add your docs here. */ public class ConstMotion { public static final double STOP = 0; public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); + public static final Angle INTAKE_PIVOT_DEPLOY = Degrees.of(90); + // jayden in the future make a intake retract/ } From 584534af8694c855dd0f2598132563763872bee4 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Sat, 23 May 2026 12:06:37 -0700 Subject: [PATCH 2/7] Added retracting intake and proceeded with binding buttons for retracting and intaking on the controller. Co-Authored-By: Tej shah <187054380+Tejshah88@users.noreply.github.com> Co-Authored-By: lameesnotlame <258483139+lameesnotlame@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 11 +++++- .../frc/robot/commands/states/Intaking.java | 1 + .../robot/commands/states/RetractIntake.java | 38 +++++++++++++++++++ .../java/frc/robot/constants/ConstMotion.java | 1 + 4 files changed, 49 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/states/RetractIntake.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 17c17cd..5ab5e1c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,8 +19,11 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.DeferredCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.DeviceIDs.controllerIDs; import frc.robot.commands.AddVisionMeasurement; +import frc.robot.commands.states.Intaking; +import frc.robot.commands.states.RetractIntake; import frc.robot.constants.ConstSystem.constControllers; import frc.robot.subsystems.DriverStateMachine; import frc.robot.subsystems.DriverStateMachine.DriverState; @@ -55,7 +58,8 @@ public class RobotContainer { private final RobotPoses loggedRobotPose = robotPose; public static final Vision subVision = new Vision(); private final Vision loggedSubVision = subVision; - + public static final Intaking intakingInstance = new Intaking(); + public static final RetractIntake RetractingInstance = new RetractIntake(); Command TRY_NONE = Commands.deferredProxy( () -> subStateMachine.tryState(RobotState.NONE)); @@ -95,7 +99,10 @@ private void configDriverBindings() { // subDrivetrain.resetModulesToAbsolute())); conDriver.btn_Back .onTrue(Commands.runOnce(() -> subDrivetrain.resetPose(new Pose2d(0, 0, new Rotation2d())))); - + conDriver.btn_RightTrigger + .whileTrue(intakingInstance); + conDriver.btn_LeftTrigger + .whileTrue(RetractingInstance); // Example Pose Drive conDriver.btn_X .whileTrue(EXAMPLE_POSE_DRIVE) diff --git a/src/main/java/frc/robot/commands/states/Intaking.java b/src/main/java/frc/robot/commands/states/Intaking.java index 463f5e8..099e6fd 100644 --- a/src/main/java/frc/robot/commands/states/Intaking.java +++ b/src/main/java/frc/robot/commands/states/Intaking.java @@ -23,6 +23,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/states/RetractIntake.java b/src/main/java/frc/robot/commands/states/RetractIntake.java new file mode 100644 index 0000000..5724441 --- /dev/null +++ b/src/main/java/frc/robot/commands/states/RetractIntake.java @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.constants.ConstMotion; + +public class RetractIntake extends Command { + /** Creates a new RetractIntake. */ + public RetractIntake() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.RETRACT_PIVOT_DEPLOY); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index be2cce3..592c11d 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -17,4 +17,5 @@ public class ConstMotion { public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final Angle INTAKE_PIVOT_DEPLOY = Degrees.of(90); // jayden in the future make a intake retract/ + public static final Angle RETRACT_PIVOT_DEPLOY = Degrees.of(-90); } From a80597b4e44a18f5c9bc8de540b2c70bf322d368 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared@users.noreply.github.com> Date: Sat, 23 May 2026 16:20:31 -0700 Subject: [PATCH 3/7] Intaking functional added intake rollers --- src/main/java/frc/robot/commands/states/Intaking.java | 5 ++++- src/main/java/frc/robot/constants/ConstRotors.java | 6 ++++-- src/main/java/frc/robot/subsystems/Rotors.java | 6 ++++-- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/states/Intaking.java b/src/main/java/frc/robot/commands/states/Intaking.java index 099e6fd..2db7a67 100644 --- a/src/main/java/frc/robot/commands/states/Intaking.java +++ b/src/main/java/frc/robot/commands/states/Intaking.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; import frc.robot.constants.ConstMotion; -import frc.robot.subsystems.Motion; +import frc.robot.constants.ConstRotors; public class Intaking extends Command { /** Creates a new Intaking. */ @@ -23,12 +23,15 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + RobotContainer.rotorsInstance.setIntakeRollersSpeeds(ConstRotors.INTAKE_ROLLERS_INTAKING); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + RobotContainer.rotorsInstance.setIntakeRollersSpeeds(ConstRotors.STOP); + } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index 88117cc..b4af895 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -7,7 +7,8 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.fasterxml.jackson.core.filter.TokenFilter.Inclusion; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; /** Add your docs here. */ public class ConstRotors { @@ -19,6 +20,7 @@ public class ConstRotors { public static final TalonFXConfiguration TRANSFER_ROLLERS_WEST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration FLYWHEEL_EAST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration FLYWHEEL_WEST_CONFIGURATION = new TalonFXConfiguration(); + public static final AngularVelocity INTAKE_ROLLERS_INTAKING = Units.RPM.of(4000); static { @@ -62,6 +64,6 @@ public class ConstRotors { } - public static final double STOP = 0; + public static final AngularVelocity STOP = Units.RPM.of(0); } diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index f62af36..09cebe5 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -4,6 +4,7 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -51,6 +52,7 @@ public Rotors() { // final MotionMagicVelocityVoltage TransferVelocityRequest = new // MotionMagicVelocityVoltage(0);/ final MotionMagicVelocityVoltage flyWheelVelocityRequest = new MotionMagicVelocityVoltage(0); + final MotionMagicVelocityVoltage intakeRollersVelocityRequest = new MotionMagicVelocityVoltage(0); public AngularVelocity getFlyWheelSpeeds() { if (Robot.isSimulation()) { @@ -75,8 +77,8 @@ public void setSerializerRollersSpeed(double speed) { serializerRollers.set(speed); } - public void setIntakeRollersSpeeds(double speed) { - intakeRollersEast.set(speed); + public void setIntakeRollersSpeeds(AngularVelocity speed) { + intakeRollersEast.setControl(intakeRollersVelocityRequest.withVelocity(speed)); intakeRollersWest.setControl(intakeRollerEastFollower); } From 47349af85ea7873e299b2c2571cb7dcd52e5f708 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared@users.noreply.github.com> Date: Sat, 23 May 2026 16:31:21 -0700 Subject: [PATCH 4/7] Intaking functionality Fixxed naming issue to be less confusing --- src/main/java/frc/robot/commands/states/RetractIntake.java | 2 +- src/main/java/frc/robot/constants/ConstMotion.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/states/RetractIntake.java b/src/main/java/frc/robot/commands/states/RetractIntake.java index 5724441..13e9907 100644 --- a/src/main/java/frc/robot/commands/states/RetractIntake.java +++ b/src/main/java/frc/robot/commands/states/RetractIntake.java @@ -17,7 +17,7 @@ public RetractIntake() { // Called when the command is initially scheduled. @Override public void initialize() { - RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.RETRACT_PIVOT_DEPLOY); + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_RETRACT); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 592c11d..171b2a3 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -16,6 +16,6 @@ public class ConstMotion { public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final Angle INTAKE_PIVOT_DEPLOY = Degrees.of(90); - // jayden in the future make a intake retract/ - public static final Angle RETRACT_PIVOT_DEPLOY = Degrees.of(-90); + // justin in the future make a intake retract/ + public static final Angle INTAKE_PIVOT_RETRACT = Degrees.of(-90); } From 339bab3f2ed2ee8b8206063005a117dca8b44a97 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared@users.noreply.github.com> Date: Sat, 23 May 2026 16:36:45 -0700 Subject: [PATCH 5/7] intake functionality moved up code --- src/main/java/frc/robot/subsystems/Rotors.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index 09cebe5..78b67d9 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -35,6 +35,9 @@ public class Rotors extends SubsystemBase { Follower transferRollersEastFollower = new Follower(intakeRollersEast.getDeviceID(), true); Follower intakeRollerEastFollower = new Follower(intakeRollersEast.getDeviceID(), true); private boolean flyWheelAtSpeed = false; + + final MotionMagicVelocityVoltage flyWheelVelocityRequest = new MotionMagicVelocityVoltage(0); + final MotionMagicVelocityVoltage intakeRollersVelocityRequest = new MotionMagicVelocityVoltage(0); // private boolean intakeRollersAtSpeed = false;/ public Rotors() { @@ -51,8 +54,6 @@ public Rotors() { // final MotionMagicVelocityVoltage TransferVelocityRequest = new // MotionMagicVelocityVoltage(0);/ - final MotionMagicVelocityVoltage flyWheelVelocityRequest = new MotionMagicVelocityVoltage(0); - final MotionMagicVelocityVoltage intakeRollersVelocityRequest = new MotionMagicVelocityVoltage(0); public AngularVelocity getFlyWheelSpeeds() { if (Robot.isSimulation()) { From a0f7df1566f1cf8080c3095318e272ab78f8c4d3 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared@users.noreply.github.com> Date: Sat, 23 May 2026 19:01:06 -0700 Subject: [PATCH 6/7] INTAKING FUNCTIONAL --- src/main/java/frc/robot/commands/states/Intaking.java | 2 +- src/main/java/frc/robot/constants/ConstRotors.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/states/Intaking.java b/src/main/java/frc/robot/commands/states/Intaking.java index 2db7a67..b1039a7 100644 --- a/src/main/java/frc/robot/commands/states/Intaking.java +++ b/src/main/java/frc/robot/commands/states/Intaking.java @@ -23,7 +23,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - RobotContainer.rotorsInstance.setIntakeRollersSpeeds(ConstRotors.INTAKE_ROLLERS_INTAKING); + RobotContainer.rotorsInstance.setIntakeRollersSpeeds(ConstRotors.INTAKE_ROLLERS_SPEED); } diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index b4af895..b727e18 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -20,7 +20,7 @@ public class ConstRotors { public static final TalonFXConfiguration TRANSFER_ROLLERS_WEST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration FLYWHEEL_EAST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration FLYWHEEL_WEST_CONFIGURATION = new TalonFXConfiguration(); - public static final AngularVelocity INTAKE_ROLLERS_INTAKING = Units.RPM.of(4000); + public static final AngularVelocity INTAKE_ROLLERS_SPEED = Units.RPM.of(4000); static { From 739ffae3ca58b3c958f6c7eeffcdf91aa73088c5 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared@users.noreply.github.com> Date: Sat, 23 May 2026 20:00:56 -0700 Subject: [PATCH 7/7] Added motion configurations --- .../java/frc/robot/constants/ConstMotion.java | 45 ++++++++++++++++--- .../java/frc/robot/constants/ConstRotors.java | 39 +++++++++++----- 2 files changed, 67 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 171b2a3..e312988 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -5,17 +5,52 @@ package frc.robot.constants; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; /** Add your docs here. */ public class ConstMotion { - public static final double STOP = 0; + // public static final double STOP = 0;/ public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); - public static final Angle INTAKE_PIVOT_DEPLOY = Degrees.of(90); - // justin in the future make a intake retract/ - public static final Angle INTAKE_PIVOT_RETRACT = Degrees.of(-90); -} + public static final Angle INTAKE_PIVOT_DEPLOY = Degrees.of(125); + public static final Angle INTAKE_PIVOT_RETRACT = Degrees.of(0); + + static { + + INTAKE_PIVOT_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Brake; + INTAKE_PIVOT_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitThreshold = INTAKE_PIVOT_DEPLOY.in(Rotations); + INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitThreshold = INTAKE_PIVOT_RETRACT.in(Rotations); + INTAKE_PIVOT_CONFIGURATION.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + INTAKE_PIVOT_CONFIGURATION.Slot0.kS = 0.3; + INTAKE_PIVOT_CONFIGURATION.Slot0.kP = 40; + INTAKE_PIVOT_CONFIGURATION.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; + INTAKE_PIVOT_CONFIGURATION.Feedback.SensorToMechanismRatio = 1.0 / ((16.0 / 42.0) * (18.0 / 60.0) * (12.0 / 60.0)); + + HOOD_PIVOT_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Brake; + HOOD_PIVOT_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + HOOD_PIVOT_CONFIGURATION.Feedback.SensorToMechanismRatio = 1.0 / ((10.0 / 42.0) * (10.0 / 210.0)); + HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Degrees.of(46).in(Rotations); + // Do not change, it's not at zero because the hood is not perfectly at 0 when + // the encoder reads 0 + HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Degrees.of(1.6).in(Rotations); + HOOD_PIVOT_CONFIGURATION.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + HOOD_PIVOT_CONFIGURATION.Slot0.kS = 0.2; + HOOD_PIVOT_CONFIGURATION.Slot0.kP = 300; + HOOD_PIVOT_CONFIGURATION.Slot0.kG = 0.1; + HOOD_PIVOT_CONFIGURATION.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index b727e18..8901a8f 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -20,7 +20,7 @@ public class ConstRotors { public static final TalonFXConfiguration TRANSFER_ROLLERS_WEST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration FLYWHEEL_EAST_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration FLYWHEEL_WEST_CONFIGURATION = new TalonFXConfiguration(); - public static final AngularVelocity INTAKE_ROLLERS_SPEED = Units.RPM.of(4000); + public static final AngularVelocity INTAKE_ROLLERS_SPEED = Units.RPM.of(3); static { @@ -28,22 +28,34 @@ public class ConstRotors { // NeutralModeValue.Coast;/ FLYWHEEL_WEST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; - FLYWHEEL_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + FLYWHEEL_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + FLYWHEEL_WEST_CONFIGURATION.Slot0.kS = 0.14; + FLYWHEEL_WEST_CONFIGURATION.Slot0.kV = 0.11167; + FLYWHEEL_WEST_CONFIGURATION.Slot0.kA = 0; + FLYWHEEL_WEST_CONFIGURATION.Slot0.kP = 0.5; + FLYWHEEL_WEST_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = 0; + FLYWHEEL_WEST_CONFIGURATION.MotionMagic.MotionMagicAcceleration = 9999; + FLYWHEEL_WEST_CONFIGURATION.MotionMagic.MotionMagicJerk = 0; FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; - FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 70; FLYWHEEL_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; FLYWHEEL_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + FLYWHEEL_EAST_CONFIGURATION.Slot0.kS = 0.14; + FLYWHEEL_EAST_CONFIGURATION.Slot0.kV = 0.11167; + FLYWHEEL_EAST_CONFIGURATION.Slot0.kA = 0; + FLYWHEEL_EAST_CONFIGURATION.Slot0.kP = 0.5; + FLYWHEEL_EAST_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = 0; + FLYWHEEL_EAST_CONFIGURATION.MotionMagic.MotionMagicAcceleration = 9999; + FLYWHEEL_EAST_CONFIGURATION.MotionMagic.MotionMagicJerk = 0; FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; - FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 70; SERIALIZER_ROLLERS_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; SERIALIZER_ROLLERS_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - SERIALIZER_ROLLERS_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; - SERIALIZER_ROLLERS_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; - INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; INTAKE_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; INTAKE_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; @@ -53,14 +65,17 @@ public class ConstRotors { INTAKE_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; - TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - TRANSFER_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; - TRANSFER_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; + TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kP = 0.7; + TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kS = 0.15; + TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kV = 0.12; + TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kA = 0; + TRANSFER_ROLLERS_EAST_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = 0; + TRANSFER_ROLLERS_EAST_CONFIGURATION.MotionMagic.MotionMagicAcceleration = 9999; + TRANSFER_ROLLERS_EAST_CONFIGURATION.MotionMagic.MotionMagicJerk = 0; TRANSFER_ROLLERS_WEST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast; TRANSFER_ROLLERS_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - TRANSFER_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; - TRANSFER_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35; }