Minor change to add manual, again, just a proof of concept, don't use this

release the kraken

.DataLogTool/datalogtool.json

@@ -0,0 +1,6 @@
+{
+  "download": {
+    "localDir": "C:\\Users\\infin\\Downloads",
+    "serverTeam": "2648"
+  }
+}

.OutlineViewer/outlineviewer.json

@@ -0,0 +1,13 @@
+{
+  "NetworkTables Settings": {
+    "mode": "Client (NT4)"
+  },
+  "transitory": {
+    "Shuffleboard": {
+      "Sensors Tab": {
+        "open": true
+      },
+      "open": true
+    }
+  }
+}

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

src/main/deploy/pathplanner/autos/One Coral Left.auto

@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to 30 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Two Coral Left.auto

@@ -0,0 +1,49 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to 30 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "30 Right to HP"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Collect Coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP to 330 Left"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/150 Right to HP.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.289041095890411,
+        "y": 2.9732020547945206
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.950171232876712,
+        "y": 1.6208904109589037
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.4424657534246574,
+        "y": 0.7944777397260271
+      },
+      "prevControl": {
+        "x": 2.389083904109589,
+        "y": 2.5374571917808213
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 54.162347045721745
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 120.96375653207336
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/30 Right to HP.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.988527397260274,
+        "y": 5.257106164383561
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.649657534252619,
+        "y": 6.474186643842743
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3072345890410957,
+        "y": 7.135316780821918
+      },
+      "prevControl": {
+        "x": 2.1636986301369863,
+        "y": 6.188698630136986
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "HP Pickup",
+      "waypointRelativePos": 0.20476190476190542,
+      "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.97262661489646
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -120.06858282186238
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/330 Right to HP.path

@@ -0,0 +1,66 @@
+{
+  "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": "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
+}

src/main/deploy/pathplanner/paths/HP to 330 Left.path

@@ -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": "HP Left Position"
+    },
+    {
+      "anchor": {
+        "x": 3.973766447368421,
+        "y": 5.246957236842105
+      },
+      "prevControl": {
+        "x": 3.6582270638067773,
+        "y": 5.772856209444845
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.5,
+      "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
+}

src/main/deploy/pathplanner/paths/Right Start to 150 Right.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.602996575342465,
+        "y": 2.0115582191780823
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.491095890410959,
+        "y": 1.9965325342465756
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.289041095890411,
+        "y": 2.9882277397260277
+      },
+      "prevControl": {
+        "x": 6.130479452054795,
+        "y": 2.11673801369863
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.7142857142857124,
+      "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": 120.25643716352937
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Start to 30 Right.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.587970890410959,
+        "y": 6.143621575342466
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.385916095890411,
+        "y": 6.158647260273973
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.988527397260274,
+        "y": 5.227054794520548
+      },
+      "prevControl": {
+        "x": 5.574529109589041,
+        "y": 6.098544520547945
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.4547619047619047,
+      "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": -121.60750224624898
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

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": []
+}

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

@@ -4,31 +4,34 @@
 
 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.constants.ElevatorConstants.ElevatorPositions;
+import frc.robot.subsystems.ManipulatorPivot;
+import frc.robot.subsystems.Vision;
 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.math.util.Units;
 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.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class RobotContainer {
-	private Arm arm;
-
 	private ClimberPivot climberPivot;
 
 	private ClimberRollers climberRollers;
@@ -36,30 +39,32 @@ public class RobotContainer {
 	private Drivetrain drivetrain;
 
 	private Elevator elevator;
-
-	private Indexer indexer;
+	//private ElevatorSysID elevator;
 
 	private Manipulator manipulator;
 
+	private ManipulatorPivot manipulatorPivot;
+
 	private CommandXboxController driver;
 	private CommandXboxController operator;
 
 	private SendableChooser<Command> autoChooser;
+	private Vision vision;
 
 	public RobotContainer() {
-		arm = new Arm();
-
 		climberPivot = new ClimberPivot();
 
 		climberRollers = new ClimberRollers();
 
+		manipulator = new Manipulator();
+
+		//vision = new Vision(drivetrain::getGyroValue);
 		drivetrain = new Drivetrain();
 
-		elevator = new Elevator();
+		elevator = new Elevator(manipulator::getAlgaePhotoswitch);
+		//elevator = new ElevatorSysID();
 
-		indexer = new Indexer();
-
-		manipulator = new Manipulator();
+		manipulatorPivot = new ManipulatorPivot();
 
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
@@ -67,19 +72,26 @@ public class RobotContainer {
 		autoChooser = AutoBuilder.buildAutoChooser();
 
 		configureButtonBindings();
+		//elevatorSysIDBindings();
 
 		configureNamedCommands();
 
 		configureShuffleboard();
 	}
 
-	private void configureButtonBindings() {
-		arm.setDefaultCommand(
-			arm.goToSetpoint(0, 1)
-		);
+	/*private void elevatorSysIDBindings() {
+		elevator.setDefaultCommand(elevator.maintainPosition());
 
+		operator.a().whileTrue(elevator.sysIdQuasistatic(Direction.kForward));
+		operator.b().whileTrue(elevator.sysIdQuasistatic(Direction.kReverse));
+		operator.x().whileTrue(elevator.sysIdDynamic(Direction.kForward));
+		operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
+	}*/
+
+	private void configureButtonBindings() {
+		//Default commands
 		climberPivot.setDefaultCommand(
-			climberPivot.goToAngle(0, 1)
+			climberPivot.runPivot(0)
 		);
 
 		climberRollers.setDefaultCommand(
@@ -94,54 +106,68 @@ public class RobotContainer {
 				() -> true
 			)
 		);
-
-		elevator.setDefaultCommand(
-			elevator.runAssistedElevator(operator::getLeftY)
-		);
-
-		indexer.setDefaultCommand(
-			indexer.runIndexer(0)
+		
+		manipulatorPivot.setDefaultCommand(
+			manipulatorPivot.maintainPosition()
 		);
+		
 
 		manipulator.setDefaultCommand(
-			manipulator.runManipulator(0)
+			manipulator.runUntilCollected(
+				() -> 0
+			)
 		);
 
 		//Driver inputs
+		
 		driver.start().whileTrue(
 			drivetrain.setXCommand()
 		);
 
 		driver.rightTrigger().whileTrue(
-			manipulator.runManipulator(1)
+			manipulator.runManipulator(() -> 1, true)
 		);
 
+		driver.leftTrigger().whileTrue( 
+			manipulator.runUntilCollected(() -> 0.75)
+		);
+
+		driver.start().and(driver.back()).onTrue(
+			startingConfig()
+		);
+		
+		driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
+		driver.povUp().whileTrue(climberPivot.runPivot(0.5));
+
+		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
+		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
+		
 		//Operator inputs
 		operator.povUp().onTrue(
-			moveManipulator(
-				ElevatorConstants.kElevatorL4Position, 
-				ArmConstants.kArmL4Position
+			safeMoveManipulator(
+				ElevatorPositions.kL4, 
+				ManipulatorPivotConstants.kL4Position
 			)
 		);
 
 		operator.povRight().onTrue(
-			moveManipulator(
-				ElevatorConstants.kElevatorL3Position, 
-				ArmConstants.kArmL3Position
+			safeMoveManipulator(
+				ElevatorPositions.kL3, 
+				ManipulatorPivotConstants.kL3Position
 			)
 		);
 
 		operator.povLeft().onTrue(
-			moveManipulator(
-				ElevatorConstants.kElevatorL2Position, 
-				ArmConstants.kArmL2Position
+			safeMoveManipulator(
+				ElevatorPositions.kL2, 
+				ManipulatorPivotConstants.kL2Position
 			)
 		);
 
 		operator.povDown().onTrue(
-			moveManipulator(
-				ElevatorConstants.kElevatorL1Position, 
-				ArmConstants.kArmL1Position
+			safeMoveManipulator(
+				ElevatorPositions.kL1, 
+				ManipulatorPivotConstants.kL1Position
 			)
 		);
 
@@ -156,6 +182,16 @@ public class RobotContainer {
 		operator.b().onTrue(
 			algaeIntakeRoutine(false)
 		);
+
+		operator.y().onTrue(
+			elevator.setTargetPosition(ElevatorPositions.kL2)
+		);
+
+		new Trigger(() -> Math.abs(MathUtil.applyDeadband(operator.getLeftY(), .05)) > .05).onTrue(
+			elevator.runManualElevator(operator::getLeftY)
+		);
+	
+
 	}
 
 	private void configureNamedCommands() {
@@ -174,84 +210,197 @@ 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)
-			.withWidget(BuiltInWidgets.kTextView);
+			.withWidget(BuiltInWidgets.kGraph);
 
-		sensorTab.addDouble("ArmPosition", arm::getEncoderPosition)
+		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
 			.withSize(2, 1)
 			.withPosition(2, 0)
 			.withWidget(BuiltInWidgets.kTextView);
-  }
+
+		sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
+			.withSize(2, 1)
+			.withPosition(2, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+			
+		sensorTab.addDouble("gyro angle", drivetrain::getGyroValue)
+			.withSize(2, 1)
+			.withPosition(0, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addBoolean("Coral Sensor", manipulator::getCoralBeamBreak)
+			.withSize(1, 1)
+			.withPosition(4, 0)
+			.withWidget(BuiltInWidgets.kBooleanBox);
+
+		sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch)
+			.withSize(1, 1)
+			.withPosition(4, 1)
+			.withWidget(BuiltInWidgets.kBooleanBox);
+
+		sensorTab.addDouble("ElevMotor1", elevator::getMotor1)
+			.withWidget(BuiltInWidgets.kGraph);
+
+		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
+			.withWidget(BuiltInWidgets.kGraph);
+
+		sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint)
+			.withSize(1, 1)
+			.withPosition(5, 0)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("Elevator error", elevator::getPIDError)
+			.withSize(1, 1)
+			.withPosition(5, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
+
+		sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
+
+
+ 	}
 
 	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
+	 */
+	@SuppressWarnings("unused")
 	private Command coralIntakeRoutine() {
 		return moveManipulator(
-			ElevatorConstants.kElevatorCoralIntakePosition, 
-			ArmConstants.kArmCoralIntakePosition
+			ElevatorPositions.kCoralIntake, 
+			ManipulatorPivotConstants.kCoralIntakePosition
 		)
-			.andThen(manipulator.runUntilCollected(1, true));
-	}
+			.andThen(manipulator.runUntilCollected(() -> .5));
+		}
 
+	/**
+	 * 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
+	 */
+	@SuppressWarnings("unused")
 	private Command algaeIntakeRoutine(boolean l2) {
 		return moveManipulator(
-			l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition, 
-			l2 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition
+			l2 ? ElevatorPositions.kL2Algae : ElevatorPositions.kL3Algae, 
+			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
 		)
-			.andThen(manipulator.runUntilCollected(1, false));
+			.andThen(manipulator.runUntilCollected(() -> 1));
 	}
 
-	private Command moveManipulator(double elevatorPosition, double armPosition) {
+	/**
+	 * 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(ElevatorPositions 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.getPosition())) || (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.kPivotSafeStowPosition, false, true)
+				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 		// 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.kPivotSafeStowPosition, false, true)
+				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 		}
 	}
 
-	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
+	/**
+	 * 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(ElevatorPositions 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.setTargetPosition(elevatorPosition).andThen(
+					new WaitUntilCommand(elevator::atSetpoint),
+					manipulatorPivot.goToSetpoint(() -> armPosition)
+				),
+				elevator.setTargetPosition(elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
 				() -> sequential
 			), 
 			Commands.either(
-				arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)), 
-				arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.setTargetPosition(elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.setTargetPosition(elevatorPosition)), 
 				() -> sequential
 			), 
 			() -> elevatorFirst
 		);
 	}
 
-	/*
-	 * A moveManipulator method that will guarantee a safe movement. 
-	 * Here in case we need want to skip moveManipulator debugging
+	@SuppressWarnings("unused")
+	private Command manipulatorSafeTravel(ElevatorPositions elevatorPosition, double armPosition, boolean isL4){
+		if(!isL4){
+			return Commands.sequence(
+				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
+				elevator.setTargetPosition(elevatorPosition),
+				new WaitUntilCommand(elevator::atSetpoint),
+				manipulatorPivot.goToSetpoint(() -> armPosition));
+			
+		}else{
+			return Commands.sequence(
+				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
+				elevator.setTargetPosition(elevatorPosition),
+				new WaitUntilCommand(() -> elevator.getEncoderPosition() > ElevatorPositions.kL4Transition.getPosition()),
+				Commands.parallel(
+					manipulatorPivot.goToSetpoint(() -> armPosition), 
+					elevator.setTargetPosition(elevatorPosition).andThen(new WaitUntilCommand(elevator::atSetpoint))
+				)
+			);
+		}
+		
+	}
+
+	/**
+	 * 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));
+	private Command safeMoveManipulator(ElevatorPositions elevatorPosition, double armPosition) {
+		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+			.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
+	}
+
+	@SuppressWarnings("unused")
+	private Command startingConfig() {
+		return moveManipulatorUtil(ElevatorPositions.kStartingPosition, 0, false, true)
+			.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
 	}
 }

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

@@ -1,79 +0,0 @@
-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 ArmConstants {
-    public static final int kArmMotorID = 0;
-    public static final int kCANcoderID = 0;
-
-    public static final int kMotorAmpsMax = 0;
-
-    public static final double kArmMaxVelocity = 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 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 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 = 0.0;
-        // TODO Need to do more reading on this setting, and how to properly offset the Arm so that horizontal is 0
-        //canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
-
-        motorConfig
-            .smartCurrentLimit(kMotorAmpsMax)
-            .idleMode(kIdleMode);
-    }
-
-
-}

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;
 

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

@@ -1,7 +1,9 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.config.SparkMaxConfig;
+
 public class ClimberPivotConstants {
-    public static final int kPivotMotorID = 0;
+    public static final int kPivotMotorID = 10;
 
     public static final int kClimberLimitSwitchID = 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 int kRollerMotorID = 9;
+
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

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

@@ -17,15 +17,15 @@ 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;
-    public static final double kFrontRightChassisAngularOffset = 0;
-    public static final double kBackLeftChassisAngularOffset = Math.PI;
-    public static final double kBackRightChassisAngularOffset = Math.PI / 2;
+    public static final double kFrontLeftChassisAngularOffset = Math.PI;
+    public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
+    public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
+    public static final double kBackRightChassisAngularOffset = 0;
 
     // 1, 7, 10 is the default for these three values
     public static final double kSysIDDrivingRampRate = 1;
@@ -38,17 +38,17 @@ public class DrivetrainConstants {
     public static final double kSysIDTurningTimeout = 10;
 
     // SPARK MAX CAN IDs
-    public static final int kFrontLeftDrivingCanId = 11;
-    public static final int kRearLeftDrivingCanId = 13;
-    public static final int kFrontRightDrivingCanId = 15;
-    public static final int kRearRightDrivingCanId = 17;
+    public static final int kFrontLeftDrivingCanId = 0;
+    public static final int kRearLeftDrivingCanId = 2;
+    public static final int kFrontRightDrivingCanId = 1;
+    public static final int kRearRightDrivingCanId = 3;
 
-    public static final int kFrontLeftTurningCanId = 10;
-    public static final int kRearLeftTurningCanId = 12;
-    public static final int kFrontRightTurningCanId = 14;
-    public static final int kRearRightTurningCanId = 16;
+    public static final int kFrontLeftTurningCanId = 2;
+    public static final int kRearLeftTurningCanId = 4;
+    public static final int kFrontRightTurningCanId = 7;
+    public static final int kRearRightTurningCanId = 5;
 
-    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
 

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

@@ -11,43 +11,73 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ElevatorConstants {
-    public static final int kElevatorMotor1ID = 0;
-    public static final int kElevatorMotor2ID = 0;
+    public enum ElevatorControlMode {
+        kPID,
+        kManualMaintain
+    }
+
+    public enum ElevatorPositions {
+        kStartingPosition(0),
+        kCoralIntake(0),
+        kL1(0),
+        kL2(14.5),
+        kL2Algae(22),
+        kL3(29),
+        kL3Algae(39),
+        kL4(53),
+        kL4Transition(40),
+        kProcessor(4);
+
+        private double position;
+
+        private ElevatorPositions(double position) {
+            this.position = position;
+        }
+
+        public double getPosition() {
+            return position;
+        }
+    }
+
+    public static final int kElevatorMotor1ID = 8;
+    public static final int kElevatorMotor2ID = 6;
 
-    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 kEncoderPositionConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0;
+    public static final double kEncoderVelocityConversionFactor = kEncoderPositionConversionFactor / 60;
 
-    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 kVelocityControllerP = 0;
-    public static final double kVelocityControllerI = 0;
-    public static final double kVelocityControllerD = 0;
+    public static final double kUpControllerP = 5.6;//7; //
+    public static final double kUpControllerI = 0;
+    public static final double kUpControllerD = 0.28;//0.28
 
-    public static final double kFeedForwardS = 0;
-    public static final double kFeedForwardG = 0;
-    public static final double kFeedForwardV = 0;
+    public static final double kDownControllerP = 5.6;//7; //
+    public static final double kDownControllerI = 0;
+    public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
+     
+    public static final double kAllowedError = 1;
 
-    public static final double kElevatorMaxVelocity = 0;
+    public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8;   /* kG too high - kG too low / 2  0.95, 0.2 */
+    public static final double kFeedForwardG = (0.95 + 0.2)/2;  /* kG too high + kG too low / 2 */    // calculated value 0.6
+    public static final double kFeedForwardV = 0.12;   // calculated value 0.12
 
-    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 kFeedForwardAlgaeHeldS = 0;
+    public static final double kFeedForwardAlgaeHeldG = 0;
+    public static final double kFeedForwardAlgaeHeldV = 0;
+
+    public static final double kMaxVelocity = 150.0;    // 120 inches per second (COOKING) calculated max is 184 in/s
+    public static final double kMaxAcceleration = 240;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+
+    /**The position of the top of the elevator brace */
+    public static final double kBracePosition = 0;
+    public static final double kMaxHeight = 47.5; //actual is 53
 
     // 1, 7, 10 are the defaults for these, change as necessary
-    public static final double kSysIDRampRate = 1;
-    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDRampRate = .25;
+    public static final double kSysIDStepVolts = 3;
     public static final double kSysIDTimeout = 10;
 
     public static final IdleMode kIdleMode = IdleMode.kBrake;
@@ -64,10 +94,11 @@ public class ElevatorConstants {
     
     static {
         motorConfig
-            .smartCurrentLimit(kMotorAmpsMax)
-            .idleMode(kIdleMode);
+            .smartCurrentLimit(kCurrentLimit)
+            .idleMode(kIdleMode)
+            .inverted(true);
         motorConfig.encoder
-            .positionConversionFactor(kEncoderConversionFactor)
-            .velocityConversionFactor(kEncoderConversionFactor / 60.0);
+            .positionConversionFactor(kEncoderPositionConversionFactor)
+            .velocityConversionFactor(kEncoderVelocityConversionFactor);
     }
 }

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,12 @@
 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 int kManipulatorMotorID = 12;
+
+    public static final int kCoralBeamBreakID = 2;
+    public static final int kAlgaePhotoswitchID = 3;
+    
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

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

@@ -0,0 +1,81 @@
+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.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 kPivotMotorID = 1;
+
+    public static final int kMotorCurrentMax = 40;
+
+    public static final double kPivotConversion = 2 * Math.PI;
+
+    public static final double kPivotMaxVelocity = 2 * Math.PI;
+
+    public static final double kPositionalP = 4;
+    public static final double kPositionalI = 0;
+    public static final double kPositionalD = 0;
+    public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
+
+    public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
+    public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
+    public static final double kFeedForwardV = 0.68; //calculated value 0.68
+
+    public static final double kFFGravityOffset = Units.degreesToRadians(135.0);
+
+    public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
+    public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
+
+    public static final double kEncoderOffset = 0.7815;
+
+    public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
+    public static final double kL1Position = Units.degreesToRadians(0.0);
+    public static final double kL2Position = Units.degreesToRadians(60.0);
+    public static final double kL3Position = Units.degreesToRadians(60.0);
+    public static final double kL4Position = Units.degreesToRadians(45.0);
+    public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
+    public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
+    public static final double kProcesserPosition = Units.degreesToRadians(175.0);
+    public static final double kNetPosition = Units.degreesToRadians(175.0);
+    /**The closest position to the elevator brace without hitting it */
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
+    /**The forward rotation limit of the pivot */
+    public static final double kRotationLimit = Units.degreesToRadians(175.0);
+
+    public static final double kSysIDRampRate = 1;
+    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDTimeout = 10;
+
+    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 SparkMaxConfig motorConfig = new SparkMaxConfig();
+
+    static {
+        motorConfig
+            .smartCurrentLimit(kMotorCurrentMax)
+            .idleMode(kIdleMode)
+            .inverted(true);
+        motorConfig.absoluteEncoder
+            .positionConversionFactor(kPivotConversion)
+            .inverted(false)
+            .zeroOffset(kEncoderOffset);
+    }
+
+
+}

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;

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

@@ -0,0 +1,31 @@
+package frc.robot.constants;
+
+public class VisionConstants {
+    // global coordinate map of all tags. index is the tag id.
+    // Units: inches and degrees. {x, y, z, z-rotation, y-rotation}
+    // This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS.
+    public static final double[][] globalTagCoords = {{},
+        {656.98, 24.73, 58.50, 126.0, 0},
+        {656.98, 291.90, 58.50, 234.0, 0},
+        {452.4, 316.21, 51.25, 270, 0},
+        {365.2, 241.44, 73.54, 0, 30},
+        {365.2, 75.19, 73.54, 0, 30},
+        {530.49, 129.97, 12.13, 300, 0},
+        {546.87, 158.3, 12.13, 0, 0},
+        {530.49, 186.63, 12.13, 60, 0},
+        {497.77, 186.63, 12.13, 120, 0},
+        {481.39, 158.3, 12.13, 180, 0},
+        {497.77, 129.97, 12.13, 240, 0},
+        {33.9, 24.73, 58.5, 54, 0},
+        {33.9, 291.9, 58.5, 306, 0},
+        {325.68, 241.44, 73.54, 180, 30},
+        {325.68, 75.19, 73.54, 180, 30},
+        {238.49, 0.42, 51.25, 90, 0},
+        {160.39, 129.97, 12.13, 240, 0},
+        {144.00, 158.3, 12.13, 180, 0},
+        {160.39, 186.63, 12.13, 120, 0},
+        {193.1, 186.63, 12.13, 60, 0},
+        {209.49, 158.3, 12.13, 0, 0},
+        {193.1, 129.97, 12.13, 300, 0},
+    };
+}

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

@@ -1,117 +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.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
-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 {
-    protected SparkMax armMotor;
-
-    private CANcoder canCoder;
-
-    private PIDController positionController;
-    private PIDController velocityController;
-
-    private ArmFeedforward feedForward;
-
-    public Arm() {
-        armMotor = new SparkMax(
-            ArmConstants.kArmMotorID, 
-            MotorType.kBrushless
-        );
-
-        armMotor.configure(ArmConstants.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-        positionController = new PIDController(
-            ArmConstants.kPositionalP, 
-            ArmConstants.kPositionalI, 
-            ArmConstants.kPositionalD
-        );
-        
-        // TODO: Generate constants for continuous input range based on CANcoder configuration?
-        positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179));
-        positionController.setTolerance(ArmConstants.kPositionalTolerance);
-
-        velocityController = new PIDController(
-            ArmConstants.kVelocityP, 
-            ArmConstants.kVelocityI, 
-            ArmConstants.kVelocityD
-        );
-
-        velocityController.setTolerance(ArmConstants.kVelocityTolerance);
-
-        canCoder = new CANcoder(ArmConstants.kCANcoderID);
-        canCoder.getConfigurator().apply(ArmConstants.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 > 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.getAbsolutePosition().getValueAsDouble());
-    }
-
-    public double getEncoderVelocity() {
-        return Units.rotationsToRadians(canCoder.getVelocity().getValueAsDouble());
-    }
-}

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

@@ -2,10 +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;
 import frc.robot.constants.ClimberPivotConstants;
@@ -15,25 +15,19 @@ public class ClimberPivot extends SubsystemBase {
 
     private RelativeEncoder neoEncoder;
 
-    private DigitalInput cageLimitSwitch;
-
-    private PIDController pidController;
-
     public ClimberPivot() {
         pivotMotor = new SparkMax(
             ClimberPivotConstants.kPivotMotorID, 
             MotorType.kBrushless
         );
 
-        neoEncoder = pivotMotor.getEncoder();
-
-        cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
-
-        pidController = new PIDController(
-            ClimberPivotConstants.kPIDControllerP,
-            ClimberPivotConstants.kPIDControllerI, 
-            ClimberPivotConstants.kPIDControllerD
+        pivotMotor.configure(
+            ClimberPivotConstants.motorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
         );
+
+        neoEncoder = pivotMotor.getEncoder();
     }
 
     public Command runPivot(double speed) {
@@ -42,18 +36,20 @@ 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);
     }
 
-    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

@@ -13,14 +13,15 @@ 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;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
@@ -35,10 +36,12 @@ public class Drivetrain extends SubsystemBase {
     protected MAXSwerveModule m_rearRight;
 
     // The gyro sensor
-    private AHRS ahrs;
+    private AHRS gyro;
 
     // Odometry class for tracking robot pose
-    private SwerveDriveOdometry m_odometry;
+    private SwerveDrivePoseEstimator m_estimator;
+
+    private Vision vision;
 
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
@@ -66,17 +69,19 @@ public class Drivetrain extends SubsystemBase {
             DrivetrainConstants.kBackRightChassisAngularOffset
         );
 
-        ahrs = new AHRS(NavXComType.kMXP_SPI);
+        gyro = new AHRS(NavXComType.kMXP_SPI);
 
-        m_odometry = new SwerveDriveOdometry(
+        m_estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(ahrs.getAngle()),
+            Rotation2d.fromDegrees(gyro.getAngle()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
-        });
+            },
+            new Pose2d()
+        );
 
         AutoBuilder.configure(
             this::getPose, 
@@ -99,7 +104,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(),
@@ -107,6 +112,25 @@ public class Drivetrain extends SubsystemBase {
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
         });
+
+
+        // if the detected tags match your alliances reef tags use their pose estimates 
+        /* 
+        if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
+            m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
+            
+        }else if(vision.getOrangeClosestTag() >= 17 || vision.getOrangeClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
+            m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
+        }
+
+        if(vision.getBlackClosestTag() >= 6 || vision.getBlackClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
+            m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp()); 
+        }else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
+            m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
+        }
+        */
+        
+        
     }
 
     public ChassisSpeeds getCurrentChassisSpeeds() {
@@ -132,7 +156,7 @@ public class Drivetrain extends SubsystemBase {
      * @return The pose.
      */
     public Pose2d getPose() {
-        return m_odometry.getPoseMeters();
+        return m_estimator.getEstimatedPosition();
     }
 
     /**
@@ -141,14 +165,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
         );
     }
@@ -233,11 +250,11 @@ public class Drivetrain extends SubsystemBase {
 
     /** Zeroes the heading of the robot. */
     public void zeroHeading() {
-        ahrs.reset();;
+        gyro.reset();
     }
 
     public double getGyroValue() {
-        return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
+        return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
     }
 
     /**
@@ -255,6 +272,10 @@ public class Drivetrain extends SubsystemBase {
      * @return The turn rate of the robot, in degrees per second
      */
     public double getTurnRate() {
-        return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
+        return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
+    }
+
+    public void addVisionMeasurement(Pose2d pose, double timestamp){
+        m_estimator.addVisionMeasurement(pose, timestamp);
     }
 }

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

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.RelativeEncoder;
@@ -8,12 +9,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.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.SubsystemBase;
 import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.ElevatorConstants.ElevatorControlMode;
+import frc.robot.constants.ElevatorConstants.ElevatorPositions;
 
 public class Elevator extends SubsystemBase {
     protected SparkMax elevatorMotor1;
@@ -21,14 +25,20 @@ public class Elevator extends SubsystemBase {
 
     protected RelativeEncoder encoder;
 
+    private BooleanSupplier hasAlgae;
+
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController positionController;
-    private PIDController velocityController;
+    private PIDController pidController;
 
     private ElevatorFeedforward feedForward;
+    private ElevatorFeedforward algaeHeldFeedForward;
+
+    private ElevatorControlMode mode;
+
+    private ElevatorPositions currentTargetPosition;
     
-    public Elevator() {
+    public Elevator(BooleanSupplier hasAlgae) {
         elevatorMotor1 = new SparkMax(
             ElevatorConstants.kElevatorMotor1ID, 
             MotorType.kBrushless
@@ -53,27 +63,64 @@ public class Elevator extends SubsystemBase {
 
         encoder = elevatorMotor1.getEncoder();
 
+        this.hasAlgae = hasAlgae;
+
         bottomLimitSwitch = new DigitalInput(
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        positionController = new PIDController(
-            ElevatorConstants.kPositionControllerP, 
-            ElevatorConstants.kPositionControllerI, 
-            ElevatorConstants.kPositionControllerD
+        pidController = new PIDController(
+            ElevatorConstants.kDownControllerP, 
+            ElevatorConstants.kDownControllerI,
+            ElevatorConstants.kDownControllerD
         );
+        pidController.setSetpoint(0);
 
-        velocityController = new PIDController(
-            ElevatorConstants.kVelocityControllerP, 
-            ElevatorConstants.kVelocityControllerI, 
-            ElevatorConstants.kVelocityControllerD
-        );
+        pidController.setTolerance(ElevatorConstants.kAllowedError);
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
             ElevatorConstants.kFeedForwardG, 
             ElevatorConstants.kFeedForwardV
         );
+
+        algaeHeldFeedForward = new ElevatorFeedforward(
+            ElevatorConstants.kFeedForwardAlgaeHeldS, 
+            ElevatorConstants.kFeedForwardAlgaeHeldG, 
+            ElevatorConstants.kFeedForwardAlgaeHeldV
+        );
+
+        mode = ElevatorControlMode.kPID;
+
+        currentTargetPosition = ElevatorPositions.kCoralIntake;
+    }
+
+    @Override
+    public void periodic() {
+        if (!getBottomLimitSwitch()) {
+            encoder.setPosition(0);
+        }
+
+        if(mode == ElevatorControlMode.kPID) {
+            if (!pidController.atSetpoint()) {
+                elevatorMotor1.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        currentTargetPosition.getPosition()
+                    ) + (hasAlgae.getAsBoolean() ? algaeHeldFeedForward.calculate(0) : feedForward.calculate(0))
+                );
+            } else {
+                if (hasAlgae.getAsBoolean()) {
+                    elevatorMotor1.setVoltage(
+                        algaeHeldFeedForward.calculate(0)
+                    );
+                } else {
+                    elevatorMotor1.setVoltage(
+                        feedForward.calculate(0)
+                    );
+                }
+            }
+        }
     }
 
     /**
@@ -94,59 +141,53 @@ 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
      * 
      * @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(() -> {
-            elevatorMotor1.set(speed);
+    public Command runManualElevator(DoubleSupplier speed) {
+        return startRun(() -> {
+            mode = ElevatorControlMode.kManualMaintain;
+        },
+        () -> {
+            double desired = speed.getAsDouble();
+
+            if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) {
+                elevatorMotor1.set(
+                    speed.getAsDouble()
+                );
+            } else {
+                if (hasAlgae.getAsBoolean()) {
+                    elevatorMotor1.setVoltage(
+                        algaeHeldFeedForward.calculate(0)
+                    );
+                } else {
+                    elevatorMotor1.setVoltage(
+                        feedForward.calculate(0)
+                    );
+                }
+            }
         });
     }
-    
-    /**
-     * Moves the elevator 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) {
-        return run(() -> {
-            double voltsOut = positionController.calculate(
-                encoder.getPosition(),
-                setpoint
-            ) + feedForward.calculate(0);
 
-            elevatorMotor1.setVoltage(voltsOut);
-        }).until(
-            () -> positionController.atSetpoint() || bottomLimitSwitch.get()
-        ).withTimeout(timeout);
+    public Command setTargetPosition(ElevatorPositions position) {
+        return runOnce(() -> {
+            if (mode == ElevatorControlMode.kManualMaintain) {
+                pidController.reset();
+                mode = ElevatorControlMode.kPID;
+            }
+
+            currentTargetPosition = position;
+        });
+    }
+
+    public boolean atSetpoint() {
+        return pidController.atSetpoint();
     }
 
     /**
@@ -158,6 +199,7 @@ public class Elevator extends SubsystemBase {
         return encoder.getPosition();
     }
 
+
     /**
      * Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
      * 
@@ -166,4 +208,30 @@ public class Elevator extends SubsystemBase {
     public boolean getBottomLimitSwitch() {
         return bottomLimitSwitch.get();
     }
+
+    /**
+     * Returns the motor's output current
+     * 
+     * @return Motor output current
+     */
+    public double getMotor1() {
+        return elevatorMotor1.getAppliedOutput()*elevatorMotor1.getBusVoltage();
+    }
+
+    /**
+     * Returns the motor's output current
+     * 
+     * @return Motor output current
+     */
+    public double getMotor2() {
+        return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
+    }
+
+    public double getPIDSetpoint() {
+        return pidController.getSetpoint();
+    }
+
+    public double getPIDError() {
+        return pidController.getError();
+    }
 }

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/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);
+    }
 }

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 algaePhotoswitch;
 
     public Manipulator() {
         manipulatorMotor = new SparkMax(
@@ -20,19 +24,76 @@ 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);
+        algaePhotoswitch = new DigitalInput(ManipulatorConstants.kAlgaePhotoswitchID);
     }
 
-    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(DoubleSupplier speed) {
         return run(() -> {
-            manipulatorMotor.set(coral ? speed : speed * -1);
-        }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
+            manipulatorMotor.set(
+                speed.getAsDouble()
+            );
+        }).until(() -> !coralBeamBreak.get());
+    }
+
+    /**
+     * Runs the manipulator in a way that will bring the coral to a reliable holding position
+     * 
+     * @return Returns a command that will position the coral to a known location
+     */
+    public Command indexCoral() {
+        return run(() -> {
+            runUntilCollected(() -> 0.5)
+                .andThen(runManipulator(() -> .1, false))
+                .until(() -> getCoralBeamBreak());
+        });
+    }
+
+    public boolean getCoralBeamBreak() {
+        return coralBeamBreak.get();
+    }
+
+    public boolean getAlgaePhotoswitch() {
+        return algaePhotoswitch.get();
     }
 }

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

@@ -0,0 +1,183 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+
+import java.util.function.DoubleSupplier;
+
+import com.revrobotics.spark.SparkAbsoluteEncoder;
+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 pivotMotor;
+
+    private SparkAbsoluteEncoder encoder;
+
+    private ArmFeedforward feedForward;
+
+    private PIDController pidController;
+
+    public ManipulatorPivot() {
+        pivotMotor = new SparkMax(
+            ManipulatorPivotConstants.kPivotMotorID, 
+            MotorType.kBrushless
+        );
+
+        pivotMotor.configure(
+            ManipulatorPivotConstants.motorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+        
+        encoder = pivotMotor.getAbsoluteEncoder();
+
+        pidController = new PIDController(
+            ManipulatorPivotConstants.kPositionalP, 
+            ManipulatorPivotConstants.kPositionalI, 
+            ManipulatorPivotConstants.kPositionalD
+        );
+        pidController.setSetpoint(0);
+
+        pidController.disableContinuousInput();
+        
+        feedForward = new ArmFeedforward(
+            ManipulatorPivotConstants.kFeedForwardS,
+            ManipulatorPivotConstants.kFeedForwardG, 
+            ManipulatorPivotConstants.kFeedForwardV
+            );
+    }
+
+    /**
+     * 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.kPivotSafeStowPosition;
+    }
+
+    /**
+     * Manual ManipulatorPivot command that sets the motor based on speed
+     * 
+     * @param speed The speed to set the motor
+     * @return A command that sets the motor speed
+     */
+    public Command runManualPivot(DoubleSupplier speed) {
+        return run(() -> {
+            pivotMotor.set(speed.getAsDouble());
+        });
+    }
+    
+    /**
+     * 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(DoubleSupplier setpoint) {
+        return startRun(() -> {
+            
+            pidController.setSetpoint(setpoint.getAsDouble());
+            pidController.reset();
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        setpoint.getAsDouble()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            }).until(() -> pidController.atSetpoint());
+    }
+
+    public Command maintainPosition() {
+        return startRun(() -> {
+            
+        
+            pidController.reset();
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        pidController.getSetpoint()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            });
+    }
+
+    /**
+     * Returns the encoder's position in radians
+     * 
+     * @return Encoder's position in radians
+     */
+    public double getEncoderPosition() {
+        return Units.radiansToDegrees( encoder.getPosition());
+    }
+    /**
+     * Returns the encoder's velocity in radians per second
+     * 
+     * @return Encoder's velocity in radians per second
+     */
+    public double getEncoderVelocity() {
+        return Units.radiansToDegrees(encoder.getVelocity());
+    }
+
+    public double getCGPosition(){
+        return Units.radiansToDegrees(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset);
+    }
+
+    public double getPivotOutput(){
+        return pivotMotor.getAppliedOutput() * pivotMotor.getBusVoltage();
+    }
+}

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

@@ -0,0 +1,144 @@
+package frc.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.constants.VisionConstants;
+
+public class Vision{
+
+    private DoubleSubscriber blackRobotRelativeX;
+    private DoubleSubscriber blackRobotRelativeY;
+    private DoubleSubscriber blackRobotRelativeZ;
+
+    private DoubleSubscriber blackClosestTag;
+    private BooleanSubscriber blackTagDetected;
+
+    private DoubleSubscriber blackFramerate;
+
+    private DoubleSubscriber orangeRobotRelativeX;
+    private DoubleSubscriber orangeRobotRelativeY;
+    private DoubleSubscriber orangeRobotRelativeZ;
+
+    private DoubleSubscriber orangeClosestTag;
+    private BooleanSubscriber orangeTagDetected;
+
+    private DoubleSubscriber orangeFramerate;
+
+    private DoubleSupplier gyroAngle;
+
+    public Vision(DoubleSupplier gyroAngle){
+        NetworkTableInstance inst = NetworkTableInstance.getDefault();
+
+        NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
+        NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
+
+        blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
+        blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
+        blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
+
+        blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
+        blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
+
+        blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
+
+        orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
+        orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
+        orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0);
+
+        orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
+        orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
+
+        orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
+    }
+    
+    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){
+        Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
+            VisionConstants.globalTagCoords[tagID][1],
+            new Rotation2d());
+
+        Pose2d relative = new Pose2d(relativeCoords, gyroAngle);
+
+        Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
+
+        Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
+
+        return new Pose2d(globalPose.getTranslation(), gyroAngle);
+    }
+
+    public Pose2d getBlackGlobalPose(){
+        return relativeToGlobalPose2d(getBlackClosestTag(), 
+            new Translation2d(getBlackRelativeX(), getBlackRelativeY()), 
+            new Rotation2d(gyroAngle.getAsDouble()));
+    }
+
+    public double getBlackRelativeX(){
+        return blackRobotRelativeX.get();
+    }
+
+    public double getBlackRelativeY(){
+        return blackRobotRelativeY.get();
+    }
+
+    public double getBlackRelativeZ(){
+        return blackRobotRelativeZ.get();
+    }
+
+    public int getBlackClosestTag(){
+        return (int) blackClosestTag.get();
+    }
+
+    public double getBlackTimeStamp(){
+        return blackRobotRelativeX.getLastChange();
+    }
+
+    public boolean getBlackTagDetected(){
+        return blackTagDetected.get();
+    }
+
+    public double getBlackFPS(){
+        return blackFramerate.get();
+    }
+
+    public Pose2d getOrangeGlobalPose(){
+        return relativeToGlobalPose2d(getOrangeClosestTag(), 
+            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()), 
+            new Rotation2d(gyroAngle.getAsDouble()));
+    }
+    
+    public double getOrangeRelativeX(){
+        return orangeRobotRelativeX.get();
+    }
+
+    public double getOrangeRelativeY(){
+        return orangeRobotRelativeY.get();
+    }
+
+    public double getOrangeRelativeZ(){
+        return orangeRobotRelativeZ.get();
+    }
+
+    public int getOrangeClosestTag(){
+        return (int) orangeClosestTag.get();
+    }
+
+    public double getOrangeTimeStamp(){
+        return orangeRobotRelativeX.getLastChange();
+    }
+
+    public boolean getOrangeTagDetected(){
+        return orangeTagDetected.get();
+    }
+
+    public double getOrangeFPS(){
+        return orangeFramerate.get();
+    }
+    
+}

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

@@ -1,8 +1,11 @@
 package frc.robot.subsystems.sysid;
 
-import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Volts;
-import static edu.wpi.first.units.Units.MetersPerSecond;
+
+import java.util.function.BooleanSupplier;
+
+import static edu.wpi.first.units.Units.InchesPerSecond;
 
 import edu.wpi.first.units.measure.MutDistance;
 import edu.wpi.first.units.measure.MutLinearVelocity;
@@ -22,14 +25,14 @@ public class ElevatorSysID extends Elevator {
 
     private SysIdRoutine routine;
 
-    public ElevatorSysID() {
-        super();
+    public ElevatorSysID(BooleanSupplier hasAlgae) {
+        super(hasAlgae);
 
         appliedVoltage = Volts.mutable(0);
 
-        elevatorPosition = Meters.mutable(0);
+        elevatorPosition = Inches.mutable(0);
 
-        elevatorVelocity = MetersPerSecond.mutable(0);
+        elevatorVelocity = InchesPerSecond.mutable(0);
 
         routine = new SysIdRoutine(
             ElevatorConstants.kSysIDConfig,
@@ -38,13 +41,13 @@ public class ElevatorSysID extends Elevator {
                 (log) -> {
                     log.motor("elevatorMotor1")
                         .voltage(appliedVoltage.mut_replace(
-                            elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
+                            getMotor1(), Volts
                         ))
                         .linearPosition(elevatorPosition.mut_replace(
-                            encoder.getPosition(), Meters
+                            encoder.getPosition(), Inches
                         ))
                         .linearVelocity(elevatorVelocity.mut_replace(
-                            encoder.getVelocity(), MetersPerSecond
+                            encoder.getVelocity(), InchesPerSecond
                         ));
                 }, 
                 this
@@ -52,6 +55,11 @@ public class ElevatorSysID extends Elevator {
         );
     }
 
+    @Override
+    public void periodic() {
+
+    }
+
     public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
         return routine.quasistatic(direction);
     }

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

@@ -10,40 +10,40 @@ 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, 
+                pivotMotor::setVoltage, 
                 (log) -> {
                     log.motor("armMotor")
                         .voltage(appliedVoltage.mut_replace(
-                            armMotor.get() * RobotController.getBatteryVoltage(), Volts
+                            pivotMotor.get() * RobotController.getBatteryVoltage(), Volts
                         ))
-                        .angularPosition(armPosition.mut_replace(
+                        .angularPosition(pivotPosition.mut_replace(
                             getEncoderPosition(), Radians
                         ))
-                        .angularVelocity(armVelocity.mut_replace(
+                        .angularVelocity(pivotVelocity.mut_replace(
                             getEncoderVelocity(), RadiansPerSecond
                         ));
                 },