Merge branch 'main' into kraken_swerve

src/main/java/frc/robot/RobotContainer.java

@@ -4,21 +4,20 @@
 
 package frc.robot;
 
-import frc.robot.constants.ManipulatorPivotConstants;
+import frc.robot.constants.ArmConstants;
-import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.OIConstants;
-import frc.robot.subsystems.ManipulatorPivot;
+import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.ClimberPivot;
 import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Manipulator;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -28,6 +27,8 @@ import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 public class RobotContainer {
+	private Arm arm;
+
 	private ClimberPivot climberPivot;
 
 	private ClimberRollers climberRollers;
@@ -36,9 +37,9 @@ public class RobotContainer {
 
 	private Elevator elevator;
 
-	private Manipulator manipulator;
+	private Indexer indexer;
 
-	private ManipulatorPivot manipulatorPivot;
+	private Manipulator manipulator;
 
 	private CommandXboxController driver;
 	private CommandXboxController operator;
@@ -46,6 +47,8 @@ public class RobotContainer {
 	private SendableChooser<Command> autoChooser;
 
 	public RobotContainer() {
+		arm = new Arm();
+
 		climberPivot = new ClimberPivot();
 
 		climberRollers = new ClimberRollers();
@@ -54,9 +57,9 @@ public class RobotContainer {
 
 		elevator = new Elevator();
 
-		manipulator = new Manipulator();
+		indexer = new Indexer();
 
-		manipulatorPivot = new ManipulatorPivot();
+		manipulator = new Manipulator();
 
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
@@ -71,9 +74,12 @@ public class RobotContainer {
 	}
 
 	private void configureButtonBindings() {
-		//Default commands
+		arm.setDefaultCommand(
+			arm.goToSetpoint(0, 1)
+		);
+
 		climberPivot.setDefaultCommand(
-			climberPivot.runPivot(0)
+			climberPivot.goToAngle(0, 1)
 		);
 
 		climberRollers.setDefaultCommand(
@@ -93,12 +99,12 @@ public class RobotContainer {
 			elevator.runAssistedElevator(operator::getLeftY)
 		);
 
-		manipulator.setDefaultCommand(
+		indexer.setDefaultCommand(
-			manipulator.defaultCommand()
+			indexer.runIndexer(0)
 		);
 
-		manipulatorPivot.setDefaultCommand(
+		manipulator.setDefaultCommand(
-			manipulatorPivot.runAssistedPivot(operator::getRightY)
+			manipulator.runManipulator(0)
 		);
 
 		//Driver inputs
@@ -107,39 +113,35 @@ public class RobotContainer {
 		);
 
 		driver.rightTrigger().whileTrue(
-			manipulator.runManipulator(() -> 1, true)
+			manipulator.runManipulator(1)
-		);
-
-		driver.start().and(driver.back()).onTrue(
-			startingConfig()
 		);
 
 		//Operator inputs
 		operator.povUp().onTrue(
 			moveManipulator(
-				ElevatorConstants.kL4Position, 
+				ElevatorConstants.kElevatorL4Position, 
-				ManipulatorPivotConstants.kL4Position
+				ArmConstants.kArmL4Position
 			)
 		);
 
 		operator.povRight().onTrue(
 			moveManipulator(
-				ElevatorConstants.kL3Position, 
+				ElevatorConstants.kElevatorL3Position, 
-				ManipulatorPivotConstants.kL3Position
+				ArmConstants.kArmL3Position
 			)
 		);
 
 		operator.povLeft().onTrue(
 			moveManipulator(
-				ElevatorConstants.kL2Position, 
+				ElevatorConstants.kElevatorL2Position, 
-				ManipulatorPivotConstants.kL2Position
+				ArmConstants.kArmL2Position
 			)
 		);
 
 		operator.povDown().onTrue(
 			moveManipulator(
-				ElevatorConstants.kL1Position, 
+				ElevatorConstants.kElevatorL1Position, 
-				ManipulatorPivotConstants.kL1Position
+				ArmConstants.kArmL1Position
 			)
 		);
 
@@ -172,140 +174,84 @@ public class RobotContainer {
 			.withPosition(0, 0)
 			.withWidget(BuiltInWidgets.kComboBoxChooser);
 
-		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
+		sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition)
 			.withSize(2, 1)
-			.withPosition(0, 1)
+			.withPosition(0, 0)
 			.withWidget(BuiltInWidgets.kTextView);
 
-		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
+		sensorTab.addDouble("ArmPosition", arm::getEncoderPosition)
-			.withSize(2, 1)
-			.withPosition(2, 1)
-			.withWidget(BuiltInWidgets.kTextView);
-
-		sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
 			.withSize(2, 1)
 			.withPosition(2, 0)
 			.withWidget(BuiltInWidgets.kTextView);
-
-		sensorTab.addBoolean("Coral Sensor", manipulator::getCoralBeamBreak)
-			.withSize(1, 1)
-			.withPosition(4, 1)
-			.withWidget(BuiltInWidgets.kBooleanBox);
-
-		sensorTab.addBoolean("Algae Sensor", manipulator::getAlgaePhotoSwitch)
-			.withSize(1, 1)
-			.withPosition(4, 0)
-			.withWidget(BuiltInWidgets.kBooleanBox);
   }
 
 	public Command getAutonomousCommand() {
 		return autoChooser.getSelected();
 	}
 
-	/**
+	//teleop routines
-	 * Moves the elevator and arm to the coral intake position, then runs the manipulator until collected
-	 * @return Moves the elevator and arm, then intakes coral
-	 */
 	private Command coralIntakeRoutine() {
 		return moveManipulator(
-			ElevatorConstants.kCoralIntakePosition, 
+			ElevatorConstants.kElevatorCoralIntakePosition, 
-			ManipulatorPivotConstants.kCoralIntakePosition
+			ArmConstants.kArmCoralIntakePosition
 		)
 			.andThen(manipulator.runUntilCollected(1, true));
 	}
 
-	/**
-	 * Moves the elevator and arm to the constant setpoints and runs the manipulator until collected
-	 * 
-	 * @param l2 Is the algae on L2? (True = L2, False = L3)
-	 * @return Moves the elevator and arm then intakes algae
-	 */
 	private Command algaeIntakeRoutine(boolean l2) {
 		return moveManipulator(
-			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
+			l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition, 
-			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
+			l2 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition
 		)
 			.andThen(manipulator.runUntilCollected(1, false));
 	}
 
-	/**
-	 * Moves the elevator and arm in different order based on target positions
-	 * 
-	 * @param elevatorPosition The target position of the elevator
-	 * @param armPosition The target rotation of the arm
-	 * @return Moves the elevator and arm to the setpoints using the most efficient path
-	 */
 	private Command moveManipulator(double elevatorPosition, double armPosition) {
 		// If the elevator current and target positions are above the brace, or the arm current and target position is in
 		// front of the brace, move together
-		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { 
+		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (arm.isMotionSafe() && arm.isMotionSafe(armPosition))) { 
 			return moveManipulatorUtil(elevatorPosition, armPosition, false, false);
 		// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
 		// then the elevator, then the arm again
-		} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { 
+		} else if (!arm.isMotionSafe(armPosition) && !arm.isMotionSafe()) { 
-			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
+			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
-				.andThen(manipulatorPivot.goToSetpoint(armPosition, 2));
+				.andThen(arm.goToSetpoint(armPosition, 2));
 		// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
-		} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
+		} else if (!arm.isMotionSafe(armPosition) && arm.isMotionSafe()) {
 			return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
 		// If the arm is behind the brace, move the arm first, then the elevator
-		} else if (!manipulatorPivot.isMotionSafe()) {
+		} else if (!arm.isMotionSafe()) {
 			return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
 		// Catch all command that's safe regardless of arm and elevator positions
 		} else { 
-			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
+			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
-				.andThen(manipulatorPivot.goToSetpoint(armPosition, 2));
+				.andThen(arm.goToSetpoint(armPosition, 2));
 		}
 	}
 
-	/**
-	 * Moves the elevator and arm in customizeable ways
-	 * 
-	 * @param elevatorPosition The target elevator position
-	 * @param armPosition The target arm position
-	 * @param elevatorFirst Does the elevator move first? (True = Elevator first, False = Arm first)
-	 * @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith)
-	 * @return Moves the elevator and arm to the setpoints
-	 */
 	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
-		if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
-			armPosition = MathUtil.clamp(
-				armPosition, 
-				0,
-				ManipulatorPivotConstants.kRotationLimit
-			);
-		}
-		
 		return Commands.either(
 			Commands.either(
-				elevator.goToSetpoint(elevatorPosition, 2).andThen(manipulatorPivot.goToSetpoint(armPosition, 2)),
+				elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)),
-				elevator.goToSetpoint(elevatorPosition, 2).alongWith(manipulatorPivot.goToSetpoint(armPosition, 2)),
+				elevator.goToSetpoint(elevatorPosition, 2).alongWith(arm.goToSetpoint(armPosition, 2)),
 				() -> sequential
 			), 
 			Commands.either(
-				manipulatorPivot.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), 
+				arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), 
-				manipulatorPivot.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), 
+				arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), 
 				() -> sequential
 			), 
 			() -> elevatorFirst
 		);
 	}
 
-	/**
+	/*
-	 * Moves the arm and elevator in a safe way.
+	 * A moveManipulator method that will guarantee a safe movement. 
-	 * 
+	 * Here in case we need want to skip moveManipulator debugging
-	 * @param elevatorPosition The target position of the elevator
-	 * @param armPosition The target rotation of the arm
-	 * @return Moves the elevator and arm to the setpoints
 	 */
 	@SuppressWarnings("unused")
 	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
-		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
+		return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
-			.andThen(manipulatorPivot.goToSetpoint(armPosition, 2));
+			.andThen(arm.goToSetpoint(armPosition, 2));
-	}
-
-	private Command startingConfig() {
-		return moveManipulatorUtil(0, 0, false, true)
-			.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
 	}
 }

src/main/java/frc/robot/constants/ArmConstants.java

@@ -13,13 +13,13 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
-public class ManipulatorPivotConstants {
+public class ArmConstants {
     public static final int kArmMotorID = 0;
     public static final int kCANcoderID = 0;
 
     public static final int kMotorAmpsMax = 0;
 
-    public static final double kPivotMaxVelocity = 0;
+    public static final double kArmMaxVelocity = 0;
 
     public static final double kPositionalP = 0;
     public static final double kPositionalI = 0;
@@ -32,17 +32,14 @@ public class ManipulatorPivotConstants {
     // TODO Is this reasonable?
     public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60;
 
-    public static final double kCoralIntakePosition = 0;
+    public static final double kArmCoralIntakePosition = 0;
-    public static final double kL1Position = 0;
+    public static final double kArmL1Position = 0;
-    public static final double kL2Position = 0;
+    public static final double kArmL2Position = 0;
-    public static final double kL3Position = 0;
+    public static final double kArmL3Position = 0;
-    public static final double kL4Position = 0;
+    public static final double kArmL4Position = 0;
-    public static final double kL2AlgaePosition = 0;
+    public static final double kArmL2AlgaePosition = 0;
-    public static final double kL3AlgaePosition = 0;
+    public static final double kArmL3AlgaePosition = 0;
-    /**The closest position to the elevator brace without hitting it */
     public static final double kArmSafeStowPosition = 0;
-    /**The forward rotation limit of the arm */
-    public static final double kRotationLimit = 0;
 
     public static final double kMagnetOffset = 0.0;
     public static final double kAbsoluteSensorDiscontinuityPoint = 0.0;
@@ -69,7 +66,7 @@ public class ManipulatorPivotConstants {
 
     static {
         canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
-        canCoderConfig.MagnetSensor.MagnetOffset = kMagnetOffset;
+        canCoderConfig.MagnetSensor.MagnetOffset = 0.0;
         // TODO Need to do more reading on this setting, and how to properly offset the Arm so that horizontal is 0
         //canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
 

src/main/java/frc/robot/constants/ClimberPivotConstants.java

@@ -1,7 +1,5 @@
 package frc.robot.constants;
 
-import com.revrobotics.spark.config.SparkMaxConfig;
-
 public class ClimberPivotConstants {
     public static final int kPivotMotorID = 0;
 
@@ -12,9 +10,4 @@ public class ClimberPivotConstants {
     public static final double kPIDControllerP = 0;
     public static final double kPIDControllerI = 0;
     public static final double kPIDControllerD = 0;
-
-    public static final double kClimberClimbPosition = 0;
-    public static final double kClimberStartingPosition = 0;
-
-    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

src/main/java/frc/robot/constants/ClimberRollersConstants.java

@@ -1,9 +1,5 @@
 package frc.robot.constants;
 
-import com.revrobotics.spark.config.SparkMaxConfig;
-
 public class ClimberRollersConstants {
     public static final int kRollerMotorID = 0;
-
-    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -33,18 +33,17 @@ public class ElevatorConstants {
     public static final double kFeedForwardG = 0;
     public static final double kFeedForwardV = 0;
 
-    public static final double kMaxVelocity = 0;
+    public static final double kElevatorMaxVelocity = 0;
 
-    public static final double kCoralIntakePosition = 0;
+    public static final double kElevatorCoralIntakePosition = 0;
-    public static final double kL1Position = 0;
+    public static final double kElevatorL1Position = 0;
-    public static final double kL2Position = 0;
+    public static final double kElevatorL2Position = 0;
-    public static final double kL3Position = 0;
+    public static final double kElevatorL3Position = 0;
-    public static final double kL4Position = 0;
+    public static final double kElevatorL4Position = 0;
-    public static final double kL2AlgaePosition = 0;
+    public static final double kElevatorL2AlgaePosition = 0;
-    public static final double kL3AlgaePosition = 0;
+    public static final double kElevatorL3AlgaePosition = 0;
-    /**The position of the top of the elevator brace */
+    public static final double kElevatorBracePosition = 0;
-    public static final double kBracePosition = 0;
+    public static final double kElevatorMaxHeight = 0;
-    public static final double kMaxHeight = 0;
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = 1;

src/main/java/frc/robot/constants/IndexerConstants.java

@@ -0,0 +1,6 @@
+package frc.robot.constants;
+
+public class IndexerConstants {
+    public static final int kIndexerMotorID = 0;
+    public static final int kIndexerBeamBreakID = 0;
+}

src/main/java/frc/robot/constants/ManipulatorConstants.java

@@ -1,11 +1,7 @@
 package frc.robot.constants;
 
-import com.revrobotics.spark.config.SparkMaxConfig;
-
 public class ManipulatorConstants {
     public static final int kManipulatorMotorID = 0;
     public static final int kCoralBeamBreakID = 0;
     public static final int kAlgaeBeamBreakID = 0;
-
-    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -2,6 +2,12 @@ package frc.robot.constants;
 
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ModuleConstants {
@@ -20,52 +26,73 @@ public class ModuleConstants {
     public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
         / kDrivingMotorReduction;
 
-    public static final int kDriveMotorCurrentLimit = 40;
+    public static final double kDrivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+    public static final double kTurningFactor = 2 * Math.PI;
+    public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
+    
+    public static final double kDriveP = .04;
+    public static final double kDriveI = 0;
+    public static final double kDriveD = 0;
+    public static final double kDriveS = 0;
+    public static final double kDriveV = kDrivingVelocityFeedForward;
+    public static final double kDriveA = 0;
+    
+    public static final double kTurnP = 1;
+    public static final double kTurnI = 0;
+    public static final double kTurnD = 0;
+
+    public static final int kDriveMotorStatorCurrentLimit = 120;
     public static final int kTurnMotorCurrentLimit = 20;
 
+    public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
+
+    public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
+    public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
+
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM 
 
-    public static final SparkMaxConfig drivingConfig = new SparkMaxConfig();
     public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
 
-    static {
+    public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
-        // Use module constants to calculate conversion factors and feed forward gain.
+    public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
-        double drivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+    public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
-        double turningFactor = 2 * Math.PI;
+    public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
-        double drivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
 
-        drivingConfig
+    static {
-            .idleMode(IdleMode.kBrake)
+        kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
-            .smartCurrentLimit(kDriveMotorCurrentLimit);
+
-        drivingConfig.encoder
+        kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true;
-            .positionConversionFactor(drivingFactor) // meters
+        kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit;
-            .velocityConversionFactor(drivingFactor / 60.0); // meters per second
+
-        drivingConfig.closedLoop
+        kDriveMotorConfig.Inverted = kDriveInversionState;
-            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+        kDriveMotorConfig.NeutralMode = kDriveIdleMode;
-            // These are example gains you may need to them for your own robot!
+
-            .pid(0.04, 0, 0)
+        kDriveSlot0Config.kP = kDriveP;
-            .velocityFF(drivingVelocityFeedForward)
+        kDriveSlot0Config.kI = kDriveI;
-            .outputRange(-1, 1);
+        kDriveSlot0Config.kD = kDriveD;
+        kDriveSlot0Config.kS = kDriveS;
+        kDriveSlot0Config.kV = kDriveV;
+        kDriveSlot0Config.kA = kDriveA;
 
         turningConfig
-            .idleMode(IdleMode.kBrake)
+            .idleMode(kTurnIdleMode)
-            .smartCurrentLimit(20);
+            .smartCurrentLimit(kTurnMotorCurrentLimit);
         turningConfig.absoluteEncoder
             // Invert the turning encoder, since the output shaft rotates in the opposite
             // direction of the steering motor in the MAXSwerve Module.
             .inverted(true)
-            .positionConversionFactor(turningFactor) // radians
+            .positionConversionFactor(kTurningFactor) // radians
-            .velocityConversionFactor(turningFactor / 60.0); // radians per second
+            .velocityConversionFactor(kTurningFactor / 60.0); // radians per second
         turningConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             // These are example gains you may need to them for your own robot!
-            .pid(1, 0, 0)
+            .pid(kTurnP, kTurnI, kTurnD)
             .outputRange(-1, 1)
             // Enable PID wrap around for the turning motor. This will allow the PID
             // controller to go through 0 to get to the setpoint i.e. going from 350 degrees
             // to 10 degrees will go through 0 rather than the other direction which is a
             // longer route.
             .positionWrappingEnabled(true)
-            .positionWrappingInputRange(0, turningFactor);
+            .positionWrappingInputRange(0, kTurningFactor);
     }
 }

src/main/java/frc/robot/subsystems/Arm.java

@@ -8,15 +8,14 @@ import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.ManipulatorPivotConstants;
+import frc.robot.constants.ArmConstants;
 
-public class ManipulatorPivot extends SubsystemBase {
+public class Arm extends SubsystemBase {
     protected SparkMax armMotor;
 
     private CANcoder canCoder;
@@ -26,38 +25,34 @@ public class ManipulatorPivot extends SubsystemBase {
 
     private ArmFeedforward feedForward;
 
-    public ManipulatorPivot() {
+    public Arm() {
         armMotor = new SparkMax(
-            ManipulatorPivotConstants.kArmMotorID, 
+            ArmConstants.kArmMotorID, 
             MotorType.kBrushless
         );
 
-        armMotor.configure(
+        armMotor.configure(ArmConstants.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-            ManipulatorPivotConstants.motorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
 
         positionController = new PIDController(
-            ManipulatorPivotConstants.kPositionalP, 
+            ArmConstants.kPositionalP, 
-            ManipulatorPivotConstants.kPositionalI, 
+            ArmConstants.kPositionalI, 
-            ManipulatorPivotConstants.kPositionalD
+            ArmConstants.kPositionalD
         );
         
         // TODO: Generate constants for continuous input range based on CANcoder configuration?
         positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179));
-        positionController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
+        positionController.setTolerance(ArmConstants.kPositionalTolerance);
 
         velocityController = new PIDController(
-            ManipulatorPivotConstants.kVelocityP, 
+            ArmConstants.kVelocityP, 
-            ManipulatorPivotConstants.kVelocityI, 
+            ArmConstants.kVelocityI, 
-            ManipulatorPivotConstants.kVelocityD
+            ArmConstants.kVelocityD
         );
 
-        velocityController.setTolerance(ManipulatorPivotConstants.kVelocityTolerance);
+        velocityController.setTolerance(ArmConstants.kVelocityTolerance);
 
-        canCoder = new CANcoder(ManipulatorPivotConstants.kCANcoderID);
+        canCoder = new CANcoder(ArmConstants.kCANcoderID);
-        canCoder.getConfigurator().apply(ManipulatorPivotConstants.canCoderConfig);
+        canCoder.getConfigurator().apply(ArmConstants.canCoderConfig);
     }
 
     /**
@@ -78,24 +73,13 @@ public class ManipulatorPivot extends SubsystemBase {
      * @return Is the motion safe
      */
     public boolean isMotionSafe(double motionTarget) {
-        return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition;
+        return motionTarget > ArmConstants.kArmSafeStowPosition;
     }
 
-    /**
+    //manual command that keeps ouput speed consistent no matter the direction
-     * A manual rotation command that will move the elevator using a consistent velocity disregarding direction
+    public Command runArm(DoubleSupplier speed) {
-     * 
-     * @param speed The velocity at which the arm rotates
-     * @return Sets motor voltage to achieve the target velocity
-     */
-    public Command runAssistedPivot(DoubleSupplier speed) {
-        double clampedSpeed = MathUtil.clamp(
-            speed.getAsDouble(), 
-            -1, 
-            1
-        );
-
         return run(() -> {
-            double realSpeedTarget = clampedSpeed * ManipulatorPivotConstants.kPivotMaxVelocity;
+            double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
 
             double voltsOut = velocityController.calculate(
                 getEncoderVelocity(), 
@@ -109,24 +93,11 @@ public class ManipulatorPivot extends SubsystemBase {
         });
     }
 
-    /**
-     * Moves the arm to a target destination (setpoint)
-     * 
-     * @param setpoint Target destination of the subsystem
-     * @param timeout Time to achieve the setpoint before quitting
-     * @return Sets motor voltage to achieve the target destination
-     */
     public Command goToSetpoint(double setpoint, double timeout) {
-        double clampedSetpoint = MathUtil.clamp(
-            setpoint, 
-            0, 
-            ManipulatorPivotConstants.kRotationLimit
-        );
-
         return run(() -> {
             double voltsOut = positionController.calculate(
                 getEncoderPosition(), 
-                clampedSetpoint
+                setpoint
             ) + feedForward.calculate(
                 getEncoderPosition(), 
                 getEncoderVelocity()
@@ -136,20 +107,10 @@ public class ManipulatorPivot extends SubsystemBase {
         }).until(positionController::atSetpoint).withTimeout(timeout);
     }
 
-    /**
-     * Returns the CANCoder's position in radians
-     * 
-     * @return CANCoder's position in radians
-     */
     public double getEncoderPosition() {
         return Units.rotationsToRadians(canCoder.getAbsolutePosition().getValueAsDouble());
     }
 
-    /**
-     * Returns the CANCoder's velocity in radians per second
-     * 
-     * @return CANCoder's velocity in radians per second
-     */
     public double getEncoderVelocity() {
         return Units.rotationsToRadians(canCoder.getVelocity().getValueAsDouble());
     }

src/main/java/frc/robot/subsystems/ClimberPivot.java

@@ -2,10 +2,9 @@ package frc.robot.subsystems;
 
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -18,21 +17,23 @@ public class ClimberPivot extends SubsystemBase {
 
     private DigitalInput cageLimitSwitch;
 
+    private PIDController pidController;
+
     public ClimberPivot() {
         pivotMotor = new SparkMax(
             ClimberPivotConstants.kPivotMotorID, 
             MotorType.kBrushless
         );
 
-        pivotMotor.configure(
-            ClimberPivotConstants.motorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
         neoEncoder = pivotMotor.getEncoder();
 
         cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
+
+        pidController = new PIDController(
+            ClimberPivotConstants.kPIDControllerP,
+            ClimberPivotConstants.kPIDControllerI, 
+            ClimberPivotConstants.kPIDControllerD
+        );
     }
 
     public Command runPivot(double speed) {
@@ -41,29 +42,18 @@ public class ClimberPivot extends SubsystemBase {
         });
     }
 
-    /**
+    public Command goToAngle(double setpoint, double timeout) {
-     * Runs the climber until it is at setpoint
-     * 
-     * @param speed The speed at which the pivot runs
-     * @param setpoint The target position of the climber
-     * @return Sets the motor speed until at the target position
-     */
-    public Command climb(double setpoint, double speed) {
         return run(() -> {
-            pivotMotor.set(speed);
+            pivotMotor.set(
-        }).until(() -> neoEncoder.getPosition() >= setpoint);
+                pidController.calculate(
+                    neoEncoder.getPosition(), 
+                    setpoint
+                )
+            );
+        }).withTimeout(timeout);
     }
 
-    /**
-     * Returns the limit switch attached to the climber. Detects if the cage is present
-     * 
-     * @return Is the cage in the climber
-     */
     public boolean getCageLimitSwitch() {
         return cageLimitSwitch.get();
     }
-
-    public double getEncoderPosition() {
-        return neoEncoder.getPosition();
-    }
 }

src/main/java/frc/robot/subsystems/ClimberRollers.java

@@ -1,15 +1,12 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ClimberRollersConstants;
 
-//TODO Figure out a way to detect if we're at the top of the cage
 public class ClimberRollers extends SubsystemBase {
     private SparkMax rollerMotor;
 
@@ -18,20 +15,8 @@ public class ClimberRollers extends SubsystemBase {
             ClimberRollersConstants.kRollerMotorID, 
             MotorType.kBrushless
         );
-
-        rollerMotor.configure(
-            ClimberRollersConstants.motorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
     }
 
-    /**
-     * Runs the rollers at a set speed
-     * 
-     * @param speed The speed in which the roller runs
-     * @return Runs the rollers at a set speed
-     */
     public Command runRoller(double speed) {
         return run(() -> {
             rollerMotor.set(speed);

src/main/java/frc/robot/subsystems/Elevator.java

@@ -8,12 +8,10 @@ import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ElevatorConstants;
 
@@ -78,13 +76,6 @@ public class Elevator extends SubsystemBase {
         );
     }
 
-    @Override
-    public void periodic() {
-        if (getBottomLimitSwitch()) {
-            encoder.setPosition(0);
-        }
-    }
-
     /**
      * Returns whether or not the motion is safe relative to the encoder's current position
      * and the elevator brace position
@@ -103,7 +94,27 @@ public class Elevator extends SubsystemBase {
      * @return Is the motion safe
      */
     public boolean isMotionSafe(double motionTarget) {
-        return motionTarget > ElevatorConstants.kBracePosition;
+        return motionTarget > ElevatorConstants.kElevatorBracePosition;
+    }
+
+    /**
+     * A manual translation command that will move the elevator using a consistent velocity disregarding direction
+     * 
+     * @param speed How fast the elevator moves
+     * @return Sets motor voltage to move the elevator relative to the speed parameter
+     */
+    public Command runAssistedElevator(DoubleSupplier speed) {
+        return run(() -> {
+            double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
+
+            double voltsOut = velocityController.calculate(
+                encoder.getVelocity(), 
+                realSpeedTarget
+            ) + feedForward.calculate(realSpeedTarget);
+
+            elevatorMotor1.setVoltage(voltsOut);
+        }).until(
+            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight);
     }
 
     /**
@@ -119,68 +130,23 @@ public class Elevator extends SubsystemBase {
     }
     
     /**
-     * A manual translation command that will move the elevator using a consistent velocity disregarding direction
+     * Moves the elevator to a target destination (setpoint)
-     * 
-     * @param speed How fast the elevator moves
-     * @return Sets motor voltage to move the elevator relative to the speed parameter
-     */
-    public Command runAssistedElevator(DoubleSupplier speed) {
-        return run(() -> {
-            double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kMaxVelocity;
-
-            double voltsOut = velocityController.calculate(
-                encoder.getVelocity(), 
-                realSpeedTarget
-            ) + feedForward.calculate(realSpeedTarget);
-
-            elevatorMotor1.setVoltage(voltsOut);
-        }).until(
-            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
-    }
-    
-    /**
-     * Moves the elevator to a target destination (setpoint). 
-     * If the setpoint is 0, the elevator will creep down to hit the limit switch
      * 
      * @param setpoint Target destination of the subsystem
      * @param timeout Time to achieve the setpoint before quitting
      * @return Sets motor voltage to achieve the target destination
      */
     public Command goToSetpoint(double setpoint, double timeout) {
-        double clampedSetpoint = MathUtil.clamp(
+        return run(() -> {
-            setpoint, 
+            double voltsOut = positionController.calculate(
-            0, 
+                encoder.getPosition(),
-            ElevatorConstants.kMaxHeight
+                setpoint
-        );
+            ) + feedForward.calculate(0);
 
-        if (clampedSetpoint == 0) {
+            elevatorMotor1.setVoltage(voltsOut);
-            return run(() -> {
+        }).until(
-                double voltsOut = positionController.calculate(
+            () -> positionController.atSetpoint() || bottomLimitSwitch.get()
-                    encoder.getPosition(),
+        ).withTimeout(timeout);
-                    clampedSetpoint
-                ) + feedForward.calculate(0);
-    
-                elevatorMotor1.setVoltage(voltsOut);
-            }).until(
-                () -> positionController.atSetpoint() || bottomLimitSwitch.get()
-            ).withTimeout(timeout)
-                .andThen(Commands.either(
-                    runAssistedElevator(() -> 0),
-                    runAssistedElevator(() -> -.2), 
-                    bottomLimitSwitch::get
-                )).withTimeout(timeout);
-        } else {
-            return run(() -> {
-                double voltsOut = positionController.calculate(
-                    encoder.getPosition(),
-                    clampedSetpoint
-                ) + feedForward.calculate(0);
-    
-                elevatorMotor1.setVoltage(voltsOut);
-            }).until(
-                () -> positionController.atSetpoint() || bottomLimitSwitch.get()
-            ).withTimeout(timeout);
-        }
     }
 
     /**

src/main/java/frc/robot/subsystems/Indexer.java

@@ -0,0 +1,36 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IndexerConstants;
+
+public class Indexer extends SubsystemBase {
+    private SparkMax indexerMotor;
+
+    private DigitalInput indexerBeamBreak;
+
+    public Indexer() {
+        indexerMotor = new SparkMax(
+            IndexerConstants.kIndexerMotorID, 
+            MotorType.kBrushless
+        );
+
+        indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakID);
+    }
+
+    public Command runIndexer(double speed) {
+        return run(() -> {
+            indexerMotor.set(speed);
+        });
+    }
+
+    public Command indexCoral(double speed) {
+        return run(() -> {
+            indexerMotor.set(speed);
+        }).until(indexerBeamBreak::get);
+    }
+}

src/main/java/frc/robot/subsystems/MAXSwerveModule.java

@@ -15,21 +15,22 @@ import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.RelativeEncoder;
 
 import frc.robot.constants.ModuleConstants;
 
 public class MAXSwerveModule {
-  private final SparkMax m_drivingSpark;
+  private final TalonFX m_drive;
   private final SparkMax m_turningSpark;
 
-  private final RelativeEncoder m_drivingEncoder;
   private final AbsoluteEncoder m_turningEncoder;
 
-  private final SparkClosedLoopController m_drivingClosedLoopController;
   private final SparkClosedLoopController m_turningClosedLoopController;
 
+  private final VelocityVoltage driveVelocityRequest;
+
   private double m_chassisAngularOffset = 0;
   private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
 
@@ -40,26 +41,29 @@ public class MAXSwerveModule {
    * Encoder.
    */
   public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
-    m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless);
+    m_drive = new TalonFX(drivingCANId);
     m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
 
-    m_drivingEncoder = m_drivingSpark.getEncoder();
     m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
 
-    m_drivingClosedLoopController = m_drivingSpark.getClosedLoopController();
     m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
 
+    driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
+
     // Apply the respective configurations to the SPARKS. Reset parameters before
     // applying the configuration to bring the SPARK to a known good state. Persist
     // the settings to the SPARK to avoid losing them on a power cycle.
-    m_drivingSpark.configure(ModuleConstants.drivingConfig, ResetMode.kResetSafeParameters,
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
-        PersistMode.kPersistParameters);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+
     m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
         PersistMode.kPersistParameters);
 
     m_chassisAngularOffset = chassisAngularOffset;
     m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
-    m_drivingEncoder.setPosition(0);
+    m_drive.setPosition(0);
   }
 
   /**
@@ -70,7 +74,7 @@ public class MAXSwerveModule {
   public SwerveModuleState getState() {
     // Apply chassis angular offset to the encoder position to get the position
     // relative to the chassis.
-    return new SwerveModuleState(m_drivingEncoder.getVelocity(),
+    return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
         new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
   }
 
@@ -82,8 +86,7 @@ public class MAXSwerveModule {
   public SwerveModulePosition getPosition() {
     // Apply chassis angular offset to the encoder position to get the position
     // relative to the chassis.
-    return new SwerveModulePosition(
+    return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
-        m_drivingEncoder.getPosition(),
         new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
   }
 
@@ -102,14 +105,21 @@ public class MAXSwerveModule {
     correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
 
     // Command driving and turning SPARKS towards their respective setpoints.
-    m_drivingClosedLoopController.setReference(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity);
+    m_drive.setControl(
+      driveVelocityRequest.withVelocity(
+        correctedDesiredState.speedMetersPerSecond
+      ).withFeedForward(
+        correctedDesiredState.speedMetersPerSecond
+      )
+    );
+
     m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);
 
     m_desiredState = desiredState;
   }
 
   public void setVoltageDrive(double voltage){
-    m_drivingSpark.setVoltage(voltage);
+    m_drive.setVoltage(voltage);
   }
 
   public void setVoltageTurn(double voltage) {
@@ -117,7 +127,7 @@ public class MAXSwerveModule {
   }
 
   public double getVoltageDrive() {
-    return m_drivingSpark.get() * RobotController.getBatteryVoltage();
+    return m_drive.get() * RobotController.getBatteryVoltage();
   }
 
   public double getVoltageTurn() {
@@ -126,6 +136,6 @@ public class MAXSwerveModule {
 
   /** Zeroes all the SwerveModule encoders. */
   public void resetEncoders() {
-    m_drivingEncoder.setPosition(0);
+    m_drive.setPosition(0);
   }
 }

src/main/java/frc/robot/subsystems/Manipulator.java

@@ -1,10 +1,6 @@
 package frc.robot.subsystems;
 
-import java.util.function.DoubleSupplier;
-
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -24,65 +20,19 @@ public class Manipulator extends SubsystemBase {
             MotorType.kBrushless
         );
 
-        manipulatorMotor.configure(
-            ManipulatorConstants.motorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
         coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
         algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
     }
 
-    /**
+    public Command runManipulator(double speed) {
-     * The default command for the manipulator that either stops the manipulator or slowly 
-     * runs the manipulator to retain the algae
-     * 
-     * @return Returns a command that sets the speed of the motor
-     */
-    public Command defaultCommand() {
         return run(() -> {
-            manipulatorMotor.set(
+            manipulatorMotor.set(speed);
-                algaeBeamBreak.get() ? 0.1 : 0
-            );
         });
     }
 
-    /**
-     * Runs the manipulator at a set speed with the direction based on the coral parameter
-     * 
-     * @param speed The speed at which the manipulator runs
-     * @param coral Is the manipulator manipulating a coral? (True = Coral, False = Algae)
-     * @return Returns a command that sets the speed of the motor
-     */
-    public Command runManipulator(DoubleSupplier speed, boolean coral) {
-        return run(() -> {
-            manipulatorMotor.set(
-                coral ? speed.getAsDouble() : speed.getAsDouble() * -1
-            );
-        });
-    }
-
-    /**
-     * Runs the manipulator until either the algae or coral beam break reads true
-     * 
-     * @param speed The speed at which the manipulator is run
-     * @param coral Is the object a coral? (True = Coral, False = Algae)
-     * @return Returns a command that sets the speed of the motor
-     */
     public Command runUntilCollected(double speed, boolean coral) {
         return run(() -> {
-            manipulatorMotor.set(
+            manipulatorMotor.set(coral ? speed : speed * -1);
-                coral ? speed : speed * -1
-            );
         }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
     }
-
-    public boolean getCoralBeamBreak() {
-        return coralBeamBreak.get();
-    }
-
-    public boolean getAlgaePhotoSwitch() {
-        return algaeBeamBreak.get();
-    }
 }

src/main/java/frc/robot/subsystems/sysid/ArmSysID.java

@@ -10,29 +10,29 @@ import edu.wpi.first.units.measure.MutVoltage;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import frc.robot.constants.ManipulatorPivotConstants;
+import frc.robot.constants.ArmConstants;
-import frc.robot.subsystems.ManipulatorPivot;
+import frc.robot.subsystems.Arm;
 
-public class ManipulatorPivotSysID extends ManipulatorPivot {
+public class ArmSysID extends Arm {
     private MutVoltage appliedVoltage;
 
-    private MutAngle pivotPosition;
+    private MutAngle armPosition;
 
-    private MutAngularVelocity pivotVelocity;
+    private MutAngularVelocity armVelocity;
 
     private SysIdRoutine routine;
 
-    public ManipulatorPivotSysID() {
+    public ArmSysID() {
         super();
 
         appliedVoltage = Volts.mutable(0);
 
-        pivotPosition = Radians.mutable(0);
+        armPosition = Radians.mutable(0);
 
-        pivotVelocity = RadiansPerSecond.mutable(0);
+        armVelocity = RadiansPerSecond.mutable(0);
 
         routine = new SysIdRoutine(
-            ManipulatorPivotConstants.kSysIDConfig,
+            ArmConstants.kSysIDConfig,
             new SysIdRoutine.Mechanism(
                 armMotor::setVoltage, 
                 (log) -> {
@@ -40,10 +40,10 @@ public class ManipulatorPivotSysID extends ManipulatorPivot {
                         .voltage(appliedVoltage.mut_replace(
                             armMotor.get() * RobotController.getBatteryVoltage(), Volts
                         ))
-                        .angularPosition(pivotPosition.mut_replace(
+                        .angularPosition(armPosition.mut_replace(
                             getEncoderPosition(), Radians
                         ))
-                        .angularVelocity(pivotVelocity.mut_replace(
+                        .angularVelocity(armVelocity.mut_replace(
                             getEncoderVelocity(), RadiansPerSecond
                         ));
                 },