Added a few shuffle board things

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

@@ -4,27 +4,30 @@
 
 package frc.robot;
 
-import frc.robot.constants.ArmConstants;
+import frc.robot.constants.ManipulatorPivotConstants;
+import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.OIConstants;
-import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.ManipulatorPivot;
 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 public class RobotContainer {
-	private Arm arm;
-
 	private ClimberPivot climberPivot;
 
 	private ClimberRollers climberRollers;
@@ -33,16 +36,16 @@ public class RobotContainer {
 
 	private Elevator elevator;
 
-	private Indexer indexer;
-
 	private Manipulator manipulator;
 
+	private ManipulatorPivot manipulatorPivot;
+
 	private CommandXboxController driver;
 	private CommandXboxController operator;
 
-	public RobotContainer() {
+	private SendableChooser<Command> autoChooser;
-		arm = new Arm();
 
+	public RobotContainer() {
 		climberPivot = new ClimberPivot();
 
 		climberRollers = new ClimberRollers();
@@ -51,25 +54,26 @@ public class RobotContainer {
 
 		elevator = new Elevator();
 
-		indexer = new Indexer();
-
 		manipulator = new Manipulator();
 
+		manipulatorPivot = new ManipulatorPivot();
+
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
+		autoChooser = AutoBuilder.buildAutoChooser();
+
 		configureButtonBindings();
 
+		configureNamedCommands();
+
 		configureShuffleboard();
 	}
 
 	private void configureButtonBindings() {
-		arm.setDefaultCommand(
+		//Default commands
-			arm.goToSetpoint(0, 1)
-		);
-
 		climberPivot.setDefaultCommand(
-			climberPivot.goToAngle(0, 1)
+			climberPivot.runPivot(0)
 		);
 
 		climberRollers.setDefaultCommand(
@@ -86,15 +90,15 @@ public class RobotContainer {
 		);
 
 		elevator.setDefaultCommand(
-			elevator.runElevator(operator::getLeftY)
+			elevator.runAssistedElevator(operator::getLeftY)
-		);
-
-		indexer.setDefaultCommand(
-			indexer.runIndexer(0)
 		);
 
 		manipulator.setDefaultCommand(
-			manipulator.runManipulator(0)
+			manipulator.defaultCommand()
+		);
+
+		manipulatorPivot.setDefaultCommand(
+			manipulatorPivot.runAssistedPivot(operator::getRightY)
 		);
 
 		//Driver inputs
@@ -103,35 +107,39 @@ public class RobotContainer {
 		);
 
 		driver.rightTrigger().whileTrue(
-			manipulator.runManipulator(1)
+			manipulator.runManipulator(() -> 1, true)
+		);
+
+		driver.start().and(driver.back()).onTrue(
+			startingConfig()
 		);
 
 		//Operator inputs
 		operator.povUp().onTrue(
 			moveManipulator(
-				ElevatorConstants.kElevatorL4Position, 
+				ElevatorConstants.kL4Position, 
-				ArmConstants.kArmL4Position
+				ManipulatorPivotConstants.kL4Position
 			)
 		);
 
 		operator.povRight().onTrue(
 			moveManipulator(
-				ElevatorConstants.kElevatorL3Position, 
+				ElevatorConstants.kL3Position, 
-				ArmConstants.kArmL3Position
+				ManipulatorPivotConstants.kL3Position
 			)
 		);
 
 		operator.povLeft().onTrue(
 			moveManipulator(
-				ElevatorConstants.kElevatorL2Position, 
+				ElevatorConstants.kL2Position, 
-				ArmConstants.kArmL2Position
+				ManipulatorPivotConstants.kL2Position
 			)
 		);
 
 		operator.povDown().onTrue(
 			moveManipulator(
-				ElevatorConstants.kElevatorL1Position, 
+				ElevatorConstants.kL1Position, 
-				ArmConstants.kArmL1Position
+				ManipulatorPivotConstants.kL1Position
 			)
 		);
 
@@ -148,78 +156,156 @@ public class RobotContainer {
 		);
 	}
 
+	private void configureNamedCommands() {
+		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
+	}
+
 	//creates tabs and transforms them on the shuffleboard
     private void configureShuffleboard() {
+		ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
     	ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
 
+		Shuffleboard.selectTab(OIConstants.kAutoTab);
+
+		autoTab.add("Auto Selection", autoChooser)
+			.withSize(2, 1)
+			.withPosition(0, 0)
+			.withWidget(BuiltInWidgets.kComboBoxChooser);
+
 		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
 			.withSize(2, 1)
-			.withPosition(0, 0)
+			.withPosition(0, 1)
 			.withWidget(BuiltInWidgets.kTextView);
 
-		sensorTab.addDouble("ArmPosition", arm::getEncoderPosition)
+		sensorTab.addDouble("Manipulator Position", manipulatorPivot::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 new PrintCommand("NO AUTO DEFINED");
+		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.kElevatorCoralIntakePosition, 
+			ElevatorConstants.kCoralIntakePosition, 
-			ArmConstants.kArmCoralIntakePosition
+			ManipulatorPivotConstants.kCoralIntakePosition
 		)
 			.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.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition, 
+			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
-			l2 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition
+			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
 		)
 			.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)) || (arm.isMotionSafe() && arm.isMotionSafe(armPosition))) { 
+		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.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 (!arm.isMotionSafe(armPosition) && !arm.isMotionSafe()) { 
+		} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { 
-			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
+			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
-				.andThen(arm.goToSetpoint(armPosition, 2));
+				.andThen(manipulatorPivot.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 (!arm.isMotionSafe(armPosition) && arm.isMotionSafe()) {
+		} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
 			return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
 		// If the arm is behind the brace, move the arm first, then the elevator
-		} else if (!arm.isMotionSafe()) {
+		} else if (!manipulatorPivot.isMotionSafe()) {
 			return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
 		// Catch all command that's safe regardless of arm and elevator positions
 		} else { 
-			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
+			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
-				.andThen(arm.goToSetpoint(armPosition, 2));
+				.andThen(manipulatorPivot.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(arm.goToSetpoint(armPosition, 2)),
+				elevator.goToSetpoint(elevatorPosition, 2).andThen(manipulatorPivot.goToSetpoint(armPosition, 2)),
-				elevator.goToSetpoint(elevatorPosition, 2).alongWith(arm.goToSetpoint(armPosition, 2)),
+				elevator.goToSetpoint(elevatorPosition, 2).alongWith(manipulatorPivot.goToSetpoint(armPosition, 2)),
 				() -> sequential
 			), 
 			Commands.either(
-				arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), 
+				manipulatorPivot.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), 
-				arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), 
+				manipulatorPivot.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), 
 				() -> sequential
 			), 
 			() -> elevatorFirst
 		);
 	}
+
+	/**
+	 * Moves the arm and elevator in a safe way.
+	 * 
+	 * @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)
+			.andThen(manipulatorPivot.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

@@ -1,25 +0,0 @@
-package frc.robot.constants;
-
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
-
-public class ArmConstants {
-    public static final int kArmMotorID = 0;
-    public static final int kCANcoderID = 0;
-
-    public static final double kEncoderConversionFactor = 0;
-
-    public static final double kArmMaxVelocity = 0;
-
-    public static final double kArmCoralIntakePosition = 0;
-    public static final double kArmL1Position = 0;
-    public static final double kArmL2Position = 0;
-    public static final double kArmL3Position = 0;
-    public static final double kArmL4Position = 0;
-    public static final double kArmL2AlgaePosition = 0;
-    public static final double kArmL3AlgaePosition = 0;
-    public static final double kArmSafeStowPosition = 0;
-
-    public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
-
-    
-}

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

@@ -1,5 +1,13 @@
 package frc.robot.constants;
 
+import java.io.IOException;
+
+import org.json.simple.parser.ParseException;
+
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
@@ -15,4 +23,21 @@ public class AutoConstants {
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
         kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+
+    // TODO This is a constant being managed like a static rewriteable variable
+    public static RobotConfig kRobotConfig;
+
+    public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
+        new PIDConstants(kPXController, 0, 0),
+        new PIDConstants(kPYController, 0, 0)
+    );
+
+    static {
+        try {
+            kRobotConfig = RobotConfig.fromGUISettings();
+        } catch (IOException | ParseException e) {
+            System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?");
+            e.printStackTrace();
+        }
+    }
 }

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

@@ -1,5 +1,7 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.config.SparkMaxConfig;
+
 public class ClimberPivotConstants {
     public static final int kPivotMotorID = 0;
 
@@ -10,4 +12,9 @@ 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,5 +1,9 @@
 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,16 +33,18 @@ public class ElevatorConstants {
     public static final double kFeedForwardG = 0;
     public static final double kFeedForwardV = 0;
 
-    public static final double kElevatorMaxVelocity = 0;
+    public static final double kMaxVelocity = 0;
 
-    public static final double kElevatorCoralIntakePosition = 0;
+    public static final double kCoralIntakePosition = 0;
-    public static final double kElevatorL1Position = 0;
+    public static final double kL1Position = 0;
-    public static final double kElevatorL2Position = 0;
+    public static final double kL2Position = 0;
-    public static final double kElevatorL3Position = 0;
+    public static final double kL3Position = 0;
-    public static final double kElevatorL4Position = 0;
+    public static final double kL4Position = 0;
-    public static final double kElevatorL2AlgaePosition = 0;
+    public static final double kL2AlgaePosition = 0;
-    public static final double kElevatorL3AlgaePosition = 0;
+    public static final double kL3AlgaePosition = 0;
-    public static final double kElevatorBracePosition = 0;
+    /**The position of the top of the elevator brace */
+    public static final double kBracePosition = 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

@@ -1,6 +0,0 @@
-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,7 +1,11 @@
 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/ManipulatorPivotConstants.java

@@ -0,0 +1,82 @@
+package frc.robot.constants;
+
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+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 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 kPositionalP = 0;
+    public static final double kPositionalI = 0;
+    public static final double kPositionalD = 0;
+    public static final double kPositionalTolerance = Units.degreesToRadians(1);
+
+    public static final double kVelocityP = 0;
+    public static final double kVelocityI = 0;
+    public static final double kVelocityD = 0;
+    // TODO Is this reasonable?
+    public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60;
+
+    public static final double kCoralIntakePosition = 0;
+    public static final double kL1Position = 0;
+    public static final double kL2Position = 0;
+    public static final double kL3Position = 0;
+    public static final double kL4Position = 0;
+    public static final double kL2AlgaePosition = 0;
+    public static final double kL3AlgaePosition = 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;
+
+    public static final double kSysIDRampRate = 1;
+    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDTimeout = 10;
+
+    public static final SensorDirectionValue kSensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIG
+
+    public static final SysIdRoutine.Config kSysIDConfig = new Config(
+        Volts.of(kSysIDRampRate).per(Second),
+        Volts.of(kSysIDStepVolts),
+        Seconds.of(kSysIDTimeout)
+    );
+
+    public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
+
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+
+    static {
+        canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+        canCoderConfig.MagnetSensor.MagnetOffset = kMagnetOffset;
+        // 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;
+
+        motorConfig
+            .smartCurrentLimit(kMotorAmpsMax)
+            .idleMode(kIdleMode);
+    }
+
+
+}

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

@@ -6,5 +6,6 @@ public class OIConstants {
     
     public static final double kDriveDeadband = 0.05;
 
+    public static final String kAutoTab = "Auto Tab";
     public static final String kSensorsTab = "Sensors Tab";
 }

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

@@ -1,94 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.function.DoubleSupplier;
-
-import com.ctre.phoenix6.hardware.CANcoder;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-
-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.ArmConstants;
-
-public class Arm extends SubsystemBase {
-    private SparkMax armMotor;
-
-    private CANcoder canCoder;
-
-    private PIDController positionController;
-    private PIDController velocityController;
-
-    private ArmFeedforward feedForward;
-
-    public Arm() {
-        armMotor = new SparkMax(
-            ArmConstants.kArmMotorID, 
-            MotorType.kBrushless
-        );
-
-        canCoder = new CANcoder(ArmConstants.kCANcoderID);
-    }
-
-    /**
-     * Returns whether or not the motion is safe relative to the encoder's current position
-     * and the arm safe stow position
-     * 
-     * @return Is the motion safe
-     */
-    public boolean isMotionSafe() {
-        return isMotionSafe(getEncoderPosition());
-    }
-
-    /**
-     * Returns whether or not the motion is safe relative to some target position and the
-     * arm safe stow position
-     * 
-     * @param motionTarget The target position to determine the safety of
-     * @return Is the motion safe
-     */
-    public boolean isMotionSafe(double motionTarget) {
-        return motionTarget > ArmConstants.kArmSafeStowPosition;
-    }
-
-    //manual command that keeps ouput speed consistent no matter the direction
-    public Command runArm(DoubleSupplier speed) {
-        return run(() -> {
-            double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
-
-            double voltsOut = velocityController.calculate(
-                getEncoderVelocity(), 
-                realSpeedTarget
-            ) + feedForward.calculate(
-                getEncoderPosition(), 
-                getEncoderVelocity()
-            );
-
-            armMotor.setVoltage(voltsOut);
-        });
-    }
-
-    public Command goToSetpoint(double setpoint, double timeout) {
-        return run(() -> {
-            double voltsOut = positionController.calculate(
-                getEncoderPosition(), 
-                setpoint
-            ) + feedForward.calculate(
-                getEncoderPosition(), 
-                getEncoderVelocity()
-            );
-
-            armMotor.setVoltage(voltsOut);
-        }).until(positionController::atSetpoint).withTimeout(timeout);
-    }
-
-    public double getEncoderPosition() {
-        return Units.rotationsToRadians(canCoder.getPosition().getValueAsDouble());
-    }
-
-    public double getEncoderVelocity() {
-        return Units.rotationsToRadians(canCoder.getVelocity().getValueAsDouble());
-    }
-}

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

@@ -2,9 +2,10 @@ 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;
@@ -17,23 +18,21 @@ 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) {
@@ -42,18 +41,29 @@ 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(
+            pivotMotor.set(speed);
-                pidController.calculate(
+        }).until(() -> neoEncoder.getPosition() >= setpoint);
-                    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,12 +1,15 @@
 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;
 
@@ -15,8 +18,20 @@ 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/Drivetrain.java

@@ -4,9 +4,14 @@
 
 package frc.robot.subsystems;
 
+import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.studica.frc.AHRS;
+import com.studica.frc.AHRS.NavXComType;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -15,10 +20,10 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.ADIS16470_IMU;
+import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
 
@@ -30,7 +35,7 @@ public class Drivetrain extends SubsystemBase {
     protected MAXSwerveModule m_rearRight;
 
     // The gyro sensor
-    private ADIS16470_IMU m_gyro;
+    private AHRS ahrs;
 
     // Odometry class for tracking robot pose
     private SwerveDriveOdometry m_odometry;
@@ -61,11 +66,41 @@ public class Drivetrain extends SubsystemBase {
             DrivetrainConstants.kBackRightChassisAngularOffset
         );
 
-        m_gyro = new ADIS16470_IMU();
+        ahrs = new AHRS(NavXComType.kMXP_SPI);
 
         m_odometry = new SwerveDriveOdometry(
             DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            Rotation2d.fromDegrees(ahrs.getAngle()),
+            new SwerveModulePosition[] {
+                m_frontLeft.getPosition(),
+                m_frontRight.getPosition(),
+                m_rearLeft.getPosition(),
+                m_rearRight.getPosition()
+        });
+
+        AutoBuilder.configure(
+            this::getPose, 
+            this::resetOdometry, 
+            this::getCurrentChassisSpeeds, 
+            this::driveWithChassisSpeeds, 
+            AutoConstants.kPPDriveController, 
+            AutoConstants.kRobotConfig,
+            () -> {
+                Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+                    if (alliance.isPresent()) {
+                        return alliance.get() == DriverStation.Alliance.Red;
+                    }
+                    return false;
+            }, 
+            this
+        );
+    }
+
+    @Override
+    public void periodic() {
+        // Update the odometry in the periodic block
+        m_odometry.update(
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -74,17 +109,21 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
-    @Override
+    public ChassisSpeeds getCurrentChassisSpeeds() {
-    public void periodic() {
+        return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(
-        // Update the odometry in the periodic block
+            m_frontLeft.getState(),
-        m_odometry.update(
+            m_frontRight.getState(),
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            m_rearLeft.getState(),
-            new SwerveModulePosition[] {
+            m_rearRight.getState()
-                m_frontLeft.getPosition(),
+        );
-                m_frontRight.getPosition(),
+    }
-                m_rearLeft.getPosition(),
+
-                m_rearRight.getPosition()
+    public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
-        });
+        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
+        SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
+        SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
+
+        setModuleStates(newStates);
     }
 
     /**
@@ -103,7 +142,7 @@ public class Drivetrain extends SubsystemBase {
      */
     public void resetOdometry(Pose2d pose) {
         m_odometry.resetPosition(
-            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -144,7 +183,7 @@ public class Drivetrain extends SubsystemBase {
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative
                 ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
-                        Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
+                        Rotation2d.fromDegrees(getGyroValue()))
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
             swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@@ -194,7 +233,11 @@ public class Drivetrain extends SubsystemBase {
 
     /** Zeroes the heading of the robot. */
     public void zeroHeading() {
-        m_gyro.reset();
+        ahrs.reset();;
+    }
+
+    public double getGyroValue() {
+        return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
     }
 
     /**
@@ -203,7 +246,7 @@ public class Drivetrain extends SubsystemBase {
      * @return the robot's heading in degrees, from -180 to 180
      */
     public double getHeading() {
-        return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
+        return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
     }
 
     /**
@@ -212,6 +255,6 @@ public class Drivetrain extends SubsystemBase {
      * @return The turn rate of the robot, in degrees per second
      */
     public double getTurnRate() {
-        return m_gyro.getRate(IMUAxis.kZ) * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
+        return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
 }

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

@@ -8,10 +8,12 @@ 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;
 
@@ -76,6 +78,13 @@ 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
@@ -94,13 +103,30 @@ public class Elevator extends SubsystemBase {
      * @return Is the motion safe
      */
     public boolean isMotionSafe(double motionTarget) {
-        return motionTarget > ElevatorConstants.kElevatorBracePosition;
+        return motionTarget > ElevatorConstants.kBracePosition;
     }
     
-    //manual command that keeps ouput speed consistent no matter the direction
+    /**
-    public Command runElevator(DoubleSupplier speed) {
+     * A manual translation command that uses feed forward calculation to maintain position
+     * 
+     * @param speed The speed at which the elevator translates
+     * @return Sets motor voltage to translate the elevator and maintain position
+     */
+    public Command runManualElevator(double speed) {
         return run(() -> {
-            double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
+            elevatorMotor1.set(speed);
+        });
+    }
+
+    /**
+     * 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.kMaxVelocity;
 
             double voltsOut = velocityController.calculate(
                 encoder.getVelocity(), 
@@ -108,16 +134,46 @@ public class Elevator extends SubsystemBase {
             ) + feedForward.calculate(realSpeedTarget);
 
             elevatorMotor1.setVoltage(voltsOut);
-        }).until(bottomLimitSwitch::get);
+        }).until(
+            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
     }
     
-    
+    /**
-    //go to setpoint command
+     * 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(
+            setpoint, 
+            0, 
+            ElevatorConstants.kMaxHeight
+        );
+
+        if (clampedSetpoint == 0) {
             return run(() -> {
                 double voltsOut = positionController.calculate(
                     encoder.getPosition(),
-                setpoint
+                    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);
@@ -125,8 +181,23 @@ public class Elevator extends SubsystemBase {
                 () -> positionController.atSetpoint() || bottomLimitSwitch.get()
             ).withTimeout(timeout);
         }
+    }
 
+    /**
+     * Returns the current encoder position
+     * 
+     * @return Current encoder position
+     */
     public double getEncoderPosition() {
         return encoder.getPosition();
     }
+
+    /**
+     * Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
+     * 
+     * @return The value of bottomLimitSwitch
+     */
+    public boolean getBottomLimitSwitch() {
+        return bottomLimitSwitch.get();
+    }
 }

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

@@ -1,36 +0,0 @@
-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/Manipulator.java

@@ -1,6 +1,10 @@
 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;
@@ -20,19 +24,65 @@ 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(speed);
+            manipulatorMotor.set(
+                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(coral ? speed : speed * -1);
+            manipulatorMotor.set(
+                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/ManipulatorPivot.java

@@ -0,0 +1,156 @@
+package frc.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+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.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;
+
+public class ManipulatorPivot extends SubsystemBase {
+    protected SparkMax armMotor;
+
+    private CANcoder canCoder;
+
+    private PIDController positionController;
+    private PIDController velocityController;
+
+    private ArmFeedforward feedForward;
+
+    public ManipulatorPivot() {
+        armMotor = new SparkMax(
+            ManipulatorPivotConstants.kArmMotorID, 
+            MotorType.kBrushless
+        );
+
+        armMotor.configure(
+            ManipulatorPivotConstants.motorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        positionController = new PIDController(
+            ManipulatorPivotConstants.kPositionalP, 
+            ManipulatorPivotConstants.kPositionalI, 
+            ManipulatorPivotConstants.kPositionalD
+        );
+        
+        // TODO: Generate constants for continuous input range based on CANcoder configuration?
+        positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179));
+        positionController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
+
+        velocityController = new PIDController(
+            ManipulatorPivotConstants.kVelocityP, 
+            ManipulatorPivotConstants.kVelocityI, 
+            ManipulatorPivotConstants.kVelocityD
+        );
+
+        velocityController.setTolerance(ManipulatorPivotConstants.kVelocityTolerance);
+
+        canCoder = new CANcoder(ManipulatorPivotConstants.kCANcoderID);
+        canCoder.getConfigurator().apply(ManipulatorPivotConstants.canCoderConfig);
+    }
+
+    /**
+     * Returns whether or not the motion is safe relative to the encoder's current position
+     * and the arm safe stow position
+     * 
+     * @return Is the motion safe
+     */
+    public boolean isMotionSafe() {
+        return isMotionSafe(getEncoderPosition());
+    }
+
+    /**
+     * Returns whether or not the motion is safe relative to some target position and the
+     * arm safe stow position
+     * 
+     * @param motionTarget The target position to determine the safety of
+     * @return Is the motion safe
+     */
+    public boolean isMotionSafe(double motionTarget) {
+        return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition;
+    }
+
+    /**
+     * A manual rotation command that will move the elevator using a consistent velocity disregarding direction
+     * 
+     * @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 voltsOut = velocityController.calculate(
+                getEncoderVelocity(), 
+                realSpeedTarget
+            ) + feedForward.calculate(
+                getEncoderPosition(), 
+                getEncoderVelocity()
+            );
+
+            armMotor.setVoltage(voltsOut);
+        });
+    }
+
+    /**
+     * 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
+            ) + feedForward.calculate(
+                getEncoderPosition(), 
+                getEncoderVelocity()
+            );
+
+            armMotor.setVoltage(voltsOut);
+        }).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/sysid/ManipulatorPivotSysID.java

@@ -0,0 +1,62 @@
+package frc.robot.subsystems.sysid;
+
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+import edu.wpi.first.units.measure.MutAngle;
+import edu.wpi.first.units.measure.MutAngularVelocity;
+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.subsystems.ManipulatorPivot;
+
+public class ManipulatorPivotSysID extends ManipulatorPivot {
+    private MutVoltage appliedVoltage;
+
+    private MutAngle pivotPosition;
+
+    private MutAngularVelocity pivotVelocity;
+
+    private SysIdRoutine routine;
+
+    public ManipulatorPivotSysID() {
+        super();
+
+        appliedVoltage = Volts.mutable(0);
+
+        pivotPosition = Radians.mutable(0);
+
+        pivotVelocity = RadiansPerSecond.mutable(0);
+
+        routine = new SysIdRoutine(
+            ManipulatorPivotConstants.kSysIDConfig,
+            new SysIdRoutine.Mechanism(
+                armMotor::setVoltage, 
+                (log) -> {
+                    log.motor("armMotor")
+                        .voltage(appliedVoltage.mut_replace(
+                            armMotor.get() * RobotController.getBatteryVoltage(), Volts
+                        ))
+                        .angularPosition(pivotPosition.mut_replace(
+                            getEncoderPosition(), Radians
+                        ))
+                        .angularVelocity(pivotVelocity.mut_replace(
+                            getEncoderVelocity(), RadiansPerSecond
+                        ));
+                }, 
+                this
+            )
+        );
+    }
+
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return routine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return routine.dynamic(direction);
+    }
+}