removed velocity controllers on position mechanisms and added controller PID for elevator

removed algae beam break
drivetrain odometry -> pose estimator
2025-02-08 03:27:59 -05:00 · 2025-01-30 12:54:43 -05:00 · 2025-01-30 04:14:57 -05:00 · 2025-01-30 03:49:33 -05:00 · 2025-01-30 01:59:08 -05:00 · 2025-01-27 21:38:03 -05:00
24 changed files with 776 additions and 332 deletions
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
--- a/src/main/deploy/pathplanner/paths/300
+++ b/src/main/deploy/pathplanner/paths/300
@@ -0,0 +1,72 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6061643835666786,
+        "y": 5.031720890416444
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.065239726031196,
+        "y": 5.647773972598556
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2510679859194649,
+        "y": 7.0812357195448
+      },
+      "prevControl": {
+        "x": 1.6678510274010594,
+        "y": 6.6394691780780075
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.498997995991984,
+      "rotationDegrees": -52.46519085612145
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Score L4",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": null
+    },
+    {
+      "name": "HP Pickup",
+      "waypointRelativePos": 0.16666666666666663,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.98486432191523
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": true
+}
--- a/src/main/deploy/pathplanner/paths/HP
+++ b/src/main/deploy/pathplanner/paths/HP
@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3072345890410957,
+        "y": 7.135316780821918
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.5994434931506847,
+        "y": 6.909931506849315
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.026883561643835,
+        "y": 5.257106164383561
+      },
+      "prevControl": {
+        "x": 3.7113441780821916,
+        "y": 5.783005136986301
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Score L4",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.69923999693802
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "useDefaultConstraints": true
+}
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.8763,
+  "robotLength": 0.8763,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 4.0,
+  "defaultMaxAccel": 4.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 48.35,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.038,
+  "driveGearing": 4.29,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 65.0,
+  "wheelCOF": 1.1,
+  "flModuleX": 0.31115,
+  "flModuleY": 0.31115,
+  "frModuleX": 0.31115,
+  "frModuleY": -0.31115,
+  "blModuleX": -0.31115,
+  "blModuleY": 0.31115,
+  "brModuleX": -0.31115,
+  "brModuleY": -0.31115,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,20 +4,21 @@

 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;
@@ -27,8 +28,6 @@ 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;
@@ -37,18 +36,16 @@ public class RobotContainer {

 	private Elevator elevator;

-	private Indexer indexer;
-
 	private Manipulator manipulator;

+	private ManipulatorPivot manipulatorPivot;
+
 	private CommandXboxController driver;
 	private CommandXboxController operator;

 	private SendableChooser<Command> autoChooser;

 	public RobotContainer() {
-		arm = new Arm();
-
 		climberPivot = new ClimberPivot();

 		climberRollers = new ClimberRollers();
@@ -57,10 +54,10 @@ 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);

@@ -74,12 +71,9 @@ public class RobotContainer {
 	}

 	private void configureButtonBindings() {
-		arm.setDefaultCommand(
-			arm.goToSetpoint(0, 1)
-		);
-
+		//Default commands
 		climberPivot.setDefaultCommand(
-			climberPivot.goToAngle(0, 1)
+			climberPivot.runPivot(0)
 		);

 		climberRollers.setDefaultCommand(
@@ -95,53 +89,57 @@ public class RobotContainer {
 			)
 		);

-		elevator.setDefaultCommand(
-			elevator.runAssistedElevator(operator::getLeftY)
-		);
-
-		indexer.setDefaultCommand(
-			indexer.runIndexer(0)
-		);
+		//elevator.setDefaultCommand(
+			//elevator.runAssistedElevator(operator::getLeftY)
+	//	);

 		manipulator.setDefaultCommand(
-			manipulator.runManipulator(0)
+			manipulator.defaultCommand()
 		);

+		//manipulatorPivot.setDefaultCommand(
+		//	manipulatorPivot.runAssistedPivot(operator::getRightY)
+		//);
+
 		//Driver inputs
 		driver.start().whileTrue(
 			drivetrain.setXCommand()
 		);

 		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, 
-				ArmConstants.kArmL4Position
+				ElevatorConstants.kL4Position, 
+				ManipulatorPivotConstants.kL4Position
 			)
 		);

 		operator.povRight().onTrue(
 			moveManipulator(
-				ElevatorConstants.kElevatorL3Position, 
-				ArmConstants.kArmL3Position
+				ElevatorConstants.kL3Position, 
+				ManipulatorPivotConstants.kL3Position
 			)
 		);

 		operator.povLeft().onTrue(
 			moveManipulator(
-				ElevatorConstants.kElevatorL2Position, 
-				ArmConstants.kArmL2Position
+				ElevatorConstants.kL2Position, 
+				ManipulatorPivotConstants.kL2Position
 			)
 		);

 		operator.povDown().onTrue(
 			moveManipulator(
-				ElevatorConstants.kElevatorL1Position, 
-				ArmConstants.kArmL1Position
+				ElevatorConstants.kL1Position, 
+				ManipulatorPivotConstants.kL1Position
 			)
 		);

@@ -174,84 +172,142 @@ public class RobotContainer {
 			.withPosition(0, 0)
 			.withWidget(BuiltInWidgets.kComboBoxChooser);

-		sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition)
+		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 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, 
-			ArmConstants.kArmCoralIntakePosition
+			ElevatorConstants.kCoralIntakePosition, 
+			ManipulatorPivotConstants.kCoralIntakePosition
 		)
-			.andThen(manipulator.runUntilCollected(1, true));
-	}
+			.andThen(manipulator.runUntilCollected(1));
+		}

+	/**
+	 * 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 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition
+			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
+			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
 		)
-			.andThen(manipulator.runUntilCollected(1, false));
+			.andThen(manipulator.runUntilCollected(1));
 	}

+	/**
+	 * 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()) { 
-			return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
-				.andThen(arm.goToSetpoint(armPosition, 2));
+		} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { 
+			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
+				.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)
-				.andThen(arm.goToSetpoint(armPosition, 2));
+			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
+				.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).alongWith(arm.goToSetpoint(armPosition, 2)),
+				elevator.goToSetpoint(elevatorPosition, 2).andThen(manipulatorPivot.goToSetpoint(armPosition, 2)),
+				elevator.goToSetpoint(elevatorPosition, 2).alongWith(manipulatorPivot.goToSetpoint(armPosition, 2)),
 				() -> sequential
 			), 
 			Commands.either(
-				arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), 
-				arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), 
+				manipulatorPivot.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), 
+				manipulatorPivot.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), 
 				() -> sequential
 			), 
 			() -> elevatorFirst
 		);
 	}

-	/*
-	 * A moveManipulator method that will guarantee a safe movement. 
-	 * Here in case we need want to skip moveManipulator debugging
+	/**
+	 * 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, ArmConstants.kArmSafeStowPosition, false, true)
-			.andThen(arm.goToSetpoint(armPosition, 2));
+		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));
 	}
 }
--- a/src/main/java/frc/robot/constants/AutoConstants.java
+++ b/src/main/java/frc/robot/constants/AutoConstants.java
@@ -11,8 +11,8 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;

 public class AutoConstants {
-    public static final double kMaxSpeedMetersPerSecond = 3;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+    public static final double kMaxSpeedMetersPerSecond = 4;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 4;
    public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
    public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;

--- a/src/main/java/frc/robot/constants/ClimberPivotConstants.java
+++ b/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();
 }
--- a/src/main/java/frc/robot/constants/ClimberRollersConstants.java
+++ b/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();
 }
--- a/src/main/java/frc/robot/constants/DrivetrainConstants.java
+++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java
@@ -17,9 +17,9 @@ public class DrivetrainConstants {
    public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second

    // Chassis configuration
-    public static final double kTrackWidth = Units.inchesToMeters(26.5);
+    public static final double kTrackWidth = Units.inchesToMeters(24.5);
    // Distance between centers of right and left wheels on robot
-    public static final double kWheelBase = Units.inchesToMeters(26.5);
+    public static final double kWheelBase = Units.inchesToMeters(24.5);

    // Angular offsets of the modules relative to the chassis in radians
    public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
@@ -48,7 +48,7 @@ public class DrivetrainConstants {
    public static final int kFrontRightTurningCanId = 14;
    public static final int kRearRightTurningCanId = 16;

-    public static final boolean kGyroReversed = false;
+    public static final boolean kGyroReversed = true;

    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM

--- a/src/main/java/frc/robot/constants/ElevatorConstants.java
+++ b/src/main/java/frc/robot/constants/ElevatorConstants.java
@@ -14,36 +14,42 @@ public class ElevatorConstants {
    public static final int kElevatorMotor1ID = 0;
    public static final int kElevatorMotor2ID = 0;

-    public static final int kTopLimitSwitchID = 0;
    public static final int kBottomLimitSwitchID = 0;

-    public static final double kEncoderConversionFactor = 0;
+    // 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
+    public static final double kEncoderConversionFactor = 11/60 * (22*0.25) * 2;

-    public static final int kMotorAmpsMax = 0;
+    public static final int kCurrentLimit = 40;

    public static final double kPositionControllerP = 0;
    public static final double kPositionControllerI = 0;
    public static final double kPositionControllerD = 0;
+
+    public static final double kAllowedError = 0.25;
    
+    /* 
    public static final double kVelocityControllerP = 0;
    public static final double kVelocityControllerI = 0;
    public static final double kVelocityControllerD = 0;
+    */

    public static final double kFeedForwardS = 0;
-    public static final double kFeedForwardG = 0;
-    public static final double kFeedForwardV = 0;
+    public static final double kFeedForwardG = 0.53; // calculated value
+    public static final double kFeedForwardV = 4.78; // calculated value

-    public static final double kElevatorMaxVelocity = 0;
+    public static final double kMaxVelocity = 0;
+    public static final double kMaxAcceleration = 0;

-    public static final double kElevatorCoralIntakePosition = 0;
-    public static final double kElevatorL1Position = 0;
-    public static final double kElevatorL2Position = 0;
-    public static final double kElevatorL3Position = 0;
-    public static final double kElevatorL4Position = 0;
-    public static final double kElevatorL2AlgaePosition = 0;
-    public static final double kElevatorL3AlgaePosition = 0;
-    public static final double kElevatorBracePosition = 0;
-    public static final double kElevatorMaxHeight = 0;
+    public static final double kCoralIntakePosition = 0;
+    public static final double kL1Position = 0;
+    public static final double kL2Position = 14.5;
+    public static final double kL3Position = 29.0;
+    public static final double kL4Position = 53.0;
+    public static final double kL2AlgaePosition = 0;
+    public static final double kL3AlgaePosition = 0;
+    /**The position of the top of the elevator brace */
+    public static final double kBracePosition = 0;
+    public static final double kMaxHeight = 53.0;

    // 1, 7, 10 are the defaults for these, change as necessary
    public static final double kSysIDRampRate = 1;
@@ -64,10 +70,19 @@ public class ElevatorConstants {
    
    static {
        motorConfig
-            .smartCurrentLimit(kMotorAmpsMax)
+            .smartCurrentLimit(kCurrentLimit)
            .idleMode(kIdleMode);
        motorConfig.encoder
            .positionConversionFactor(kEncoderConversionFactor)
            .velocityConversionFactor(kEncoderConversionFactor / 60.0);
+        motorConfig.closedLoop
+            .p(kPositionControllerP)
+            .i(kPositionControllerI)
+            .d(kPositionControllerD)
+            .velocityFF(0.0); // keep at zero for position pid
+        motorConfig.closedLoop.maxMotion
+            .maxAcceleration(kMaxAcceleration)
+            .maxVelocity(kMaxVelocity)
+            .allowedClosedLoopError(kAllowedError);
    }
 }
--- a/src/main/java/frc/robot/constants/IndexerConstants.java
+++ b/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;
-}
--- a/src/main/java/frc/robot/constants/ManipulatorConstants.java
+++ b/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();
 }
--- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
+++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
@@ -13,13 +13,15 @@ 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 ArmConstants {
+public class ManipulatorPivotConstants {
    public static final int kArmMotorID = 0;
    public static final int kCANcoderID = 0;

-    public static final int kMotorAmpsMax = 0;
+    public static final int kMotorAmpsMax = 40;

-    public static final double kArmMaxVelocity = 0;
+    public static final double kPivotConversion = 12/60 * 20/60 * 28/12;
+
+    public static final double kPivotMaxVelocity = 0;

    public static final double kPositionalP = 0;
    public static final double kPositionalI = 0;
@@ -29,17 +31,25 @@ public class ArmConstants {
    public static final double kVelocityP = 0;
    public static final double kVelocityI = 0;
    public static final double kVelocityD = 0;
+
+    public static final double kFeedForwardS = 0;
+    public static final double kFeedForwardG = 0.41; // calculated value
+    public static final double kFeedForwardV = 0.68; //calculated value
+
    // TODO Is this reasonable?
    public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60;

-    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 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;
@@ -66,13 +76,14 @@ public class ArmConstants {

    static {
        canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
-        canCoderConfig.MagnetSensor.MagnetOffset = 0.0;
+        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);
+        motorConfig.encoder.positionConversionFactor(kPivotConversion);
    }


--- a/src/main/java/frc/robot/constants/ModuleConstants.java
+++ b/src/main/java/frc/robot/constants/ModuleConstants.java
@@ -41,7 +41,8 @@ public class ModuleConstants {
    public static final double kTurnI = 0;
    public static final double kTurnD = 0;

-    public static final int kDriveMotorStatorCurrentLimit = 120;
+    public static final int kDriveMotorStatorCurrentLimit = 100;
+    public static final int kDriveMotorSupplyCurrentLimit = 65;
    public static final int kTurnMotorCurrentLimit = 20;

    public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
@@ -62,7 +63,9 @@ public class ModuleConstants {
        kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;

        kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true;
+        kDriveCurrentLimitConfig.SupplyCurrentLimitEnable = true;
        kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit;
+        kDriveCurrentLimitConfig.SupplyCurrentLimit = kDriveMotorSupplyCurrentLimit;

        kDriveMotorConfig.Inverted = kDriveInversionState;
        kDriveMotorConfig.NeutralMode = kDriveIdleMode;
--- a/src/main/java/frc/robot/subsystems/ClimberPivot.java
+++ b/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(
-                pidController.calculate(
-                    neoEncoder.getPosition(), 
-                    setpoint
-                )
-            );
-        }).withTimeout(timeout);
+            pivotMotor.set(speed);
+        }).until(() -> neoEncoder.getPosition() >= setpoint);
    }

+    /**
+     * 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();
+    }
+}
--- a/src/main/java/frc/robot/subsystems/ClimberRollers.java
+++ b/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);
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -13,11 +13,11 @@ import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;

 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 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.DriverStation;
@@ -38,7 +38,7 @@ public class Drivetrain extends SubsystemBase {
    private AHRS ahrs;

    // Odometry class for tracking robot pose
-    private SwerveDriveOdometry m_odometry;
+    private SwerveDrivePoseEstimator m_estimator;

    /** Creates a new DriveSubsystem. */
    public Drivetrain() {
@@ -68,7 +68,7 @@ public class Drivetrain extends SubsystemBase {

        ahrs = new AHRS(NavXComType.kMXP_SPI);

-        m_odometry = new SwerveDriveOdometry(
+        m_estimator = new SwerveDrivePoseEstimator(
            DrivetrainConstants.kDriveKinematics,
            Rotation2d.fromDegrees(ahrs.getAngle()),
            new SwerveModulePosition[] {
@@ -76,7 +76,9 @@ public class Drivetrain extends SubsystemBase {
                m_frontRight.getPosition(),
                m_rearLeft.getPosition(),
                m_rearRight.getPosition()
-        });
+            },
+            new Pose2d()
+        );

        AutoBuilder.configure(
            this::getPose, 
@@ -99,7 +101,7 @@ public class Drivetrain extends SubsystemBase {
    @Override
    public void periodic() {
        // Update the odometry in the periodic block
-        m_odometry.update(
+        m_estimator.update(
            Rotation2d.fromDegrees(getGyroValue()),
            new SwerveModulePosition[] {
                m_frontLeft.getPosition(),
@@ -132,7 +134,7 @@ public class Drivetrain extends SubsystemBase {
     * @return The pose.
     */
    public Pose2d getPose() {
-        return m_odometry.getPoseMeters();
+        return m_estimator.getEstimatedPosition();
    }

    /**
@@ -141,14 +143,7 @@ public class Drivetrain extends SubsystemBase {
     * @param pose The pose to which to set the odometry.
     */
    public void resetOdometry(Pose2d pose) {
-        m_odometry.resetPosition(
-            Rotation2d.fromDegrees(getGyroValue()),
-            new SwerveModulePosition[] {
-                m_frontLeft.getPosition(),
-                m_frontRight.getPosition(),
-                m_rearLeft.getPosition(),
-                m_rearRight.getPosition()
-            },
+        m_estimator.resetPose(
            pose
        );
    }
@@ -257,4 +252,8 @@ public class Drivetrain extends SubsystemBase {
    public double getTurnRate() {
        return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
    }
+
+    public void addVisionMeasurement(Pose2d pose, double timestamp){
+        m_estimator.addVisionMeasurement(pose, timestamp);
+    }
 }
--- a/src/main/java/frc/robot/subsystems/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -3,15 +3,20 @@ package frc.robot.subsystems;
 import java.util.function.DoubleSupplier;

 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkClosedLoopController;
 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.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;

@@ -19,12 +24,14 @@ public class Elevator extends SubsystemBase {
    protected SparkMax elevatorMotor1;
    protected SparkMax elevatorMotor2;

+    private SparkClosedLoopController elevatorClosedLoop;
+
    protected RelativeEncoder encoder;

    private DigitalInput bottomLimitSwitch;

    private PIDController positionController;
-    private PIDController velocityController;
+    //private PIDController velocityController;

    private ElevatorFeedforward feedForward;
    
@@ -39,6 +46,8 @@ public class Elevator extends SubsystemBase {
            MotorType.kBrushless
        );

+        elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
+
        elevatorMotor1.configure(
            ElevatorConstants.motorConfig, 
            ResetMode.kResetSafeParameters, 
@@ -63,11 +72,13 @@ public class Elevator extends SubsystemBase {
            ElevatorConstants.kPositionControllerD
        );

+        /* 
        velocityController = new PIDController(
            ElevatorConstants.kVelocityControllerP, 
            ElevatorConstants.kVelocityControllerI, 
            ElevatorConstants.kVelocityControllerD
        );
+        */

        feedForward = new ElevatorFeedforward(
            ElevatorConstants.kFeedForwardS, 
@@ -76,6 +87,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,29 +112,9 @@ public class Elevator extends SubsystemBase {
     * @return Is the motion safe
     */
    public boolean isMotionSafe(double motionTarget) {
-        return motionTarget > ElevatorConstants.kElevatorBracePosition;
+        return motionTarget > ElevatorConstants.kBracePosition;
    }
-
-    /**
-     * 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);
-    }
-
+    
    /**
     * A manual translation command that uses feed forward calculation to maintain position
     * 
@@ -128,25 +126,83 @@ public class Elevator extends SubsystemBase {
            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(), 
+                realSpeedTarget
+            ) + feedForward.calculate(realSpeedTarget);
+
+            elevatorMotor1.setVoltage(voltsOut);
+        }).until(
+            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
+    }
+            */
    
    /**
-     * Moves the elevator to a target destination (setpoint)
+     * 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) {
-        return run(() -> {
-            double voltsOut = positionController.calculate(
-                encoder.getPosition(),
-                setpoint
-            ) + feedForward.calculate(0);
+        double clampedSetpoint = MathUtil.clamp(
+            setpoint, 
+            0, 
+            ElevatorConstants.kMaxHeight
+        );
+
+        return run(() -> {
+
+            elevatorClosedLoop.setReference(clampedSetpoint,
+                SparkBase.ControlType.kMAXMotionPositionControl,
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(0)
+                );
+                
+        });
+/* 
+        if (clampedSetpoint == 0) {
+            return run(() -> {
+                double voltsOut = positionController.calculate(
+                    encoder.getPosition(),
+                    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);
+        }
+            */

-            elevatorMotor1.setVoltage(voltsOut);
-        }).until(
-            () -> positionController.atSetpoint() || bottomLimitSwitch.get()
-        ).withTimeout(timeout);
    }

    /**
--- a/src/main/java/frc/robot/subsystems/Indexer.java
+++ b/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);
-    }
-}
--- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java
@@ -22,120 +22,120 @@ import com.revrobotics.AbsoluteEncoder;
 import frc.robot.constants.ModuleConstants;

 public class MAXSwerveModule {
-  private final TalonFX m_drive;
-  private final SparkMax m_turningSpark;
+    private final TalonFX m_drive;
+    private final SparkMax m_turningSpark;

-  private final AbsoluteEncoder m_turningEncoder;
+    private final AbsoluteEncoder m_turningEncoder;

-  private final SparkClosedLoopController m_turningClosedLoopController;
+    private final SparkClosedLoopController m_turningClosedLoopController;

-  private final VelocityVoltage driveVelocityRequest;
+    private final VelocityVoltage driveVelocityRequest;

-  private double m_chassisAngularOffset = 0;
-  private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
+    private double m_chassisAngularOffset = 0;
+    private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());

-  /**
-   * Constructs a MAXSwerveModule and configures the driving and turning motor,
-   * encoder, and PID controller. This configuration is specific to the REV
-   * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
-   * Encoder.
-   */
-  public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
-    m_drive = new TalonFX(drivingCANId);
-    m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
+    /**
+     * Constructs a MAXSwerveModule and configures the driving and turning motor,
+     * encoder, and PID controller. This configuration is specific to the REV
+     * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
+     * Encoder.
+     */
+    public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
+        m_drive = new TalonFX(drivingCANId);
+        m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);

-    m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
+        m_turningEncoder = m_turningSpark.getAbsoluteEncoder();

-    m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
+        m_turningClosedLoopController = m_turningSpark.getClosedLoopController();

-    driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
+        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_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
-    m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
-    m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
-    m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+        // 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_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
+        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_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
+            PersistMode.kPersistParameters);

-    m_chassisAngularOffset = chassisAngularOffset;
-    m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
-    m_drive.setPosition(0);
-  }
+        m_chassisAngularOffset = chassisAngularOffset;
+        m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
+        m_drive.setPosition(0);
+    }

-  /**
-   * Returns the current state of the module.
-   *
-   * @return The current state of the module.
-   */
-  public SwerveModuleState getState() {
-    // Apply chassis angular offset to the encoder position to get the position
-    // relative to the chassis.
-    return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
-        new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
-  }
+    /**
+     * Returns the current state of the module.
+     *
+     * @return The current state of the module.
+     */
+    public SwerveModuleState getState() {
+        // Apply chassis angular offset to the encoder position to get the position
+        // relative to the chassis.
+        return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
+            new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
+    }

-  /**
-   * Returns the current position of the module.
-   *
-   * @return The current position of the module.
-   */
-  public SwerveModulePosition getPosition() {
-    // Apply chassis angular offset to the encoder position to get the position
-    // relative to the chassis.
-    return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
-        new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
-  }
+    /**
+     * Returns the current position of the module.
+     *
+     * @return The current position of the module.
+     */
+    public SwerveModulePosition getPosition() {
+        // Apply chassis angular offset to the encoder position to get the position
+        // relative to the chassis.
+        return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
+            new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
+    }

-  /**
-   * Sets the desired state for the module.
-   *
-   * @param desiredState Desired state with speed and angle.
-   */
-  public void setDesiredState(SwerveModuleState desiredState) {
-    // Apply chassis angular offset to the desired state.
-    SwerveModuleState correctedDesiredState = new SwerveModuleState();
-    correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
-    correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));
+    /**
+     * Sets the desired state for the module.
+     *
+     * @param desiredState Desired state with speed and angle.
+     */
+    public void setDesiredState(SwerveModuleState desiredState) {
+        // Apply chassis angular offset to the desired state.
+        SwerveModuleState correctedDesiredState = new SwerveModuleState();
+        correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
+        correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));

-    // Optimize the reference state to avoid spinning further than 90 degrees.
-    correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
+        // Optimize the reference state to avoid spinning further than 90 degrees.
+        correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));

-    // Command driving and turning SPARKS towards their respective setpoints.
-    m_drive.setControl(
-      driveVelocityRequest.withVelocity(
-        correctedDesiredState.speedMetersPerSecond
-      ).withFeedForward(
-        correctedDesiredState.speedMetersPerSecond
-      )
-    );
+        // Command driving and turning SPARKS towards their respective setpoints.
+        m_drive.setControl(
+        driveVelocityRequest.withVelocity(
+            correctedDesiredState.speedMetersPerSecond
+        ).withFeedForward(
+            correctedDesiredState.speedMetersPerSecond
+        )
+        );

-    m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);
+        m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);

-    m_desiredState = desiredState;
-  }
+        m_desiredState = desiredState;
+    }

-  public void setVoltageDrive(double voltage){
-    m_drive.setVoltage(voltage);
-  }
+    public void setVoltageDrive(double voltage){
+        m_drive.setVoltage(voltage);
+    }

-  public void setVoltageTurn(double voltage) {
-    m_turningSpark.setVoltage(voltage);
-  }
+    public void setVoltageTurn(double voltage) {
+        m_turningSpark.setVoltage(voltage);
+    }

-  public double getVoltageDrive() {
-    return m_drive.get() * RobotController.getBatteryVoltage();
-  }
+    public double getVoltageDrive() {
+        return m_drive.get() * RobotController.getBatteryVoltage();
+    }

-  public double getVoltageTurn() {
-    return m_turningSpark.get() * RobotController.getBatteryVoltage();
-  }
+    public double getVoltageTurn() {
+        return m_turningSpark.get() * RobotController.getBatteryVoltage();
+    }

-  /** Zeroes all the SwerveModule encoders. */
-  public void resetEncoders() {
-    m_drive.setPosition(0);
-  }
+    /** Zeroes all the SwerveModule encoders. */
+    public void resetEncoders() {
+        m_drive.setPosition(0);
+    }
 }
--- a/src/main/java/frc/robot/subsystems/Manipulator.java
+++ b/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;
@@ -12,7 +16,7 @@ public class Manipulator extends SubsystemBase {
    private SparkMax manipulatorMotor;

    private DigitalInput coralBeamBreak;
-    private DigitalInput algaeBeamBreak;
+  //  private DigitalInput algaeBeamBreak; 

    public Manipulator() {
        manipulatorMotor = new SparkMax(
@@ -20,19 +24,59 @@ 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);
+      //  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);
+            runUntilCollected(0.1);
+            });
+    }
+
+    /**
+     * 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
+            );
        });
    }

-    public Command runUntilCollected(double speed, boolean coral) {
+    /**
+     * 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) {
        return run(() -> {
-            manipulatorMotor.set(coral ? speed : speed * -1);
-        }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
+            manipulatorMotor.set(
+                speed * -1
+            );
+        }).until(() -> coralBeamBreak.get());
+    }
+
+    public boolean getCoralBeamBreak() {
+        return coralBeamBreak.get();
    }
 }
--- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java
+++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java
@@ -8,14 +8,15 @@ 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.ArmConstants;
+import frc.robot.constants.ManipulatorPivotConstants;

-public class Arm extends SubsystemBase {
+public class ManipulatorPivot extends SubsystemBase {
    protected SparkMax armMotor;

    private CANcoder canCoder;
@@ -25,34 +26,44 @@ public class Arm extends SubsystemBase {

    private ArmFeedforward feedForward;

-    public Arm() {
+    public ManipulatorPivot() {
        armMotor = new SparkMax(
-            ArmConstants.kArmMotorID, 
+            ManipulatorPivotConstants.kArmMotorID, 
            MotorType.kBrushless
        );

-        armMotor.configure(ArmConstants.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        armMotor.configure(
+            ManipulatorPivotConstants.motorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );

        positionController = new PIDController(
-            ArmConstants.kPositionalP, 
-            ArmConstants.kPositionalI, 
-            ArmConstants.kPositionalD
+            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(ArmConstants.kPositionalTolerance);
+        positionController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);

        velocityController = new PIDController(
-            ArmConstants.kVelocityP, 
-            ArmConstants.kVelocityI, 
-            ArmConstants.kVelocityD
+            ManipulatorPivotConstants.kVelocityP, 
+            ManipulatorPivotConstants.kVelocityI, 
+            ManipulatorPivotConstants.kVelocityD
        );

-        velocityController.setTolerance(ArmConstants.kVelocityTolerance);
+        velocityController.setTolerance(ManipulatorPivotConstants.kVelocityTolerance);

-        canCoder = new CANcoder(ArmConstants.kCANcoderID);
-        canCoder.getConfigurator().apply(ArmConstants.canCoderConfig);
+        canCoder = new CANcoder(ManipulatorPivotConstants.kCANcoderID);
+        canCoder.getConfigurator().apply(ManipulatorPivotConstants.canCoderConfig);
+
+        feedForward = new ArmFeedforward(
+            ManipulatorPivotConstants.kFeedForwardS,
+            ManipulatorPivotConstants.kFeedForwardG, 
+            ManipulatorPivotConstants.kFeedForwardV
+            );
    }

    /**
@@ -73,13 +84,24 @@ public class Arm extends SubsystemBase {
     * @return Is the motion safe
     */
    public boolean isMotionSafe(double motionTarget) {
-        return motionTarget > ArmConstants.kArmSafeStowPosition;
+        return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition;
    }

-    //manual command that keeps ouput speed consistent no matter the direction
-    public Command runArm(DoubleSupplier speed) {
+    /**
+     * 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 = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
+            double realSpeedTarget = clampedSpeed * ManipulatorPivotConstants.kPivotMaxVelocity;

            double voltsOut = velocityController.calculate(
                getEncoderVelocity(), 
@@ -93,11 +115,24 @@ public class Arm 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(), 
-                setpoint
+                clampedSetpoint
            ) + feedForward.calculate(
                getEncoderPosition(), 
                getEncoderVelocity()
@@ -107,10 +142,20 @@ public class Arm 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());
    }
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -0,0 +1,51 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Vision{
+
+    private DoubleSubscriber cam1GlobalPose;
+    private DoubleSubscriber cam1ClosestTag;
+    private BooleanSubscriber cam1TagDetected;
+
+    private DoubleSubscriber cam1TimeStamp;
+
+    private DoubleSubscriber framerate;
+
+    public Vision(){
+        NetworkTableInstance inst = NetworkTableInstance.getDefault();
+
+        NetworkTable visionTable = inst.getTable("Fiducial");
+
+        cam1GlobalPose = visionTable.getDoubleTopic("cam1GlobalPose").subscribe(0.0);
+        cam1ClosestTag = visionTable.getDoubleTopic("cam1ClosestTag").subscribe(0.0);
+
+        cam1TagDetected = visionTable.getBooleanTopic("cam1_visible").subscribe(false);
+
+        framerate = visionTable.getDoubleTopic("fps").subscribe(0.0);
+    }
+
+    public double getCam1GlobalPose(){
+        return cam1GlobalPose.get();
+    }
+
+    public double getCam1ClosestTag(){
+        return cam1ClosestTag.get();
+    }
+
+    public boolean getCam1TagDetected(){
+        return cam1TagDetected.get();
+    }
+
+    public double getCam1TimeStamp(){
+        return cam1GlobalPose.getLastChange();
+    }
+
+    public double getFPS(){
+        return framerate.get();
+    }
+    
+}
--- a/src/main/java/frc/robot/subsystems/sysid/ManipulatorPivotSysID.java
+++ b/src/main/java/frc/robot/subsystems/sysid/ManipulatorPivotSysID.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.ArmConstants;
-import frc.robot.subsystems.Arm;
+import frc.robot.constants.ManipulatorPivotConstants;
+import frc.robot.subsystems.ManipulatorPivot;

-public class ArmSysID extends Arm {
+public class ManipulatorPivotSysID extends ManipulatorPivot {
    private MutVoltage appliedVoltage;

-    private MutAngle armPosition;
+    private MutAngle pivotPosition;

-    private MutAngularVelocity armVelocity;
+    private MutAngularVelocity pivotVelocity;

    private SysIdRoutine routine;

-    public ArmSysID() {
+    public ManipulatorPivotSysID() {
        super();

        appliedVoltage = Volts.mutable(0);

-        armPosition = Radians.mutable(0);
+        pivotPosition = Radians.mutable(0);

-        armVelocity = RadiansPerSecond.mutable(0);
+        pivotVelocity = RadiansPerSecond.mutable(0);

        routine = new SysIdRoutine(
-            ArmConstants.kSysIDConfig,
+            ManipulatorPivotConstants.kSysIDConfig,
            new SysIdRoutine.Mechanism(
                armMotor::setVoltage, 
                (log) -> {
@@ -40,10 +40,10 @@ public class ArmSysID extends Arm {
                        .voltage(appliedVoltage.mut_replace(
                            armMotor.get() * RobotController.getBatteryVoltage(), Volts
                        ))
-                        .angularPosition(armPosition.mut_replace(
+                        .angularPosition(pivotPosition.mut_replace(
                            getEncoderPosition(), Radians
                        ))
-                        .angularVelocity(armVelocity.mut_replace(
+                        .angularVelocity(pivotVelocity.mut_replace(
                            getEncoderVelocity(), RadiansPerSecond
                        ));
                },
Author	SHA1	Message	Date
Tylr-J42	56980d3772	removed velocity controllers on position mechanisms and added controller PID for elevator	2025-02-08 03:27:59 -05:00
Tylr-J42	6fa4377e52	removed algae beam break	2025-01-30 12:54:43 -05:00
Tylr-J42	89c1914d11	drivetrain odometry -> pose estimator	2025-01-30 04:14:57 -05:00
Tylr-J42	3af046f058	changing a bunch of constants and fixing stuff	2025-01-30 03:49:33 -05:00
Tylr-J42	34a547026d	added vision class	2025-01-30 01:59:08 -05:00
Tylr-J42	0e91643b57	Merge branch 'kraken_swerve' release the kraken	2025-01-27 21:38:03 -05:00
NoahLacks63	5fa4738b36	Added a few shuffle board things	2025-01-27 14:18:28 +00:00
NoahLacks63	ff3ecf6d1d	Didnt commit everything for some reason	2025-01-26 19:31:38 +00:00
NoahLacks63	cef200a864	Renamed things for consistency, added a few methods	2025-01-26 19:28:49 +00:00
NoahLacks63	dff4d0e04f	added spark configs to all subsystems, fixed a few formatting inconsistencies, added a TODO	2025-01-21 04:18:36 +00:00
NoahLacks63	9ab7ffad84	1,000 comments, reworked the climber pivot, removed indexer, added clamps on goToSetpoint methods	2025-01-21 03:56:00 +00:00