Clean Up Climber

src/main/java/frc/robot/Constants.java

@@ -25,7 +25,6 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.networktables.PubSub;
 import edu.wpi.first.wpilibj.XboxController;
 import frc.robot.util.FFConstants;
 
@@ -262,7 +261,6 @@ public static final class Module {
 
 			// Hope this works??? This should be tuned using SYSID or Power, I, and Damping method
 			public static final FFConstants REAL_FF = new FFConstants(0, 0);
-			// TODO: Test 0.0020645, 0.0, 0.0 (YAGSL configs)
 			public static final PIDConstants REAL_DRIVE_PID = new PIDConstants(0.0020645, 0.0, 0.0);
 			public static final PIDConstants REAL_TURN_PID = new PIDConstants(7.0, 0.0, 0.00007);
 
@@ -281,14 +279,8 @@ public static final class Sim {
 				public static final double TURN_MOI = 0.004;
 			}
 
-			// TODO: Put constants from those abstractions in here
-			public static final class SparkMax {}
-
-			public static final class TalonFX {}
-
 			public static final class Hybrid {
 
-				// TODO: Uh do these work?
 				public static final double DRIVE_GEAR_RATIO = 5.357;
 				public static final double TURN_GEAR_RATIO = 21.4286;
 
@@ -302,11 +294,6 @@ public static final class Hybrid {
 				public static final int SPARK_AVG_DEPTH = 2;
 				public static final double SPARK_FRAME_PERIOD = 1000.0 / ODOMETRY_FREQUENCY;
 
-				// CAN/Device IDS and offsets (May be Wrong, guessed which ids are correct off vibes)
-
-				// TODO: Confirm module IDs are associated with correct module and tune module offsets if
-				// last years dont work
-
 				// Front Left Module
 				public static final int DRIVE0_ID = 2;
 				public static final int TURN0_ID = 1;
@@ -369,15 +356,23 @@ public static final class Climber {
 		public static final int LEFT_ID = 33;
 		public static final int RIGHT_ID = 34;
 
+		public static final int CURRENT_FILTER_TAPS = 5;
+
+		public static final double ZEROING_SPEED = -0.25;
+
+		// Sim Configs
+		public static final int NUM_MOTORS = 2;
+		public static final double GEARING = 0.01;
+		public static final double JKG_M_SQUARED = 1;
+
 		// IN ROTATIONS (climber rotates a lot ig)
-		// TODO: TUNE
 		public static final double ERROR_OF_MARGIN = 1;
+		public static final double IDLE = 10;
+		public static final double CLIMBING = 150;
 
-		// TODO: Check if these are still right
 		public static final double LEFT_CURRENT_LIMIT = 13;
 		public static final double RIGHT_CURRENT_LIMIT = 13;
 
-		// TODO: TUNE
 		public static final PIDConstants REAL_PID = new PIDConstants(0.05, 0, 0);
 		public static final PIDConstants SIM_PID = new PIDConstants(1, 0, 0);
 	}

src/main/java/frc/robot/subsystems/climbers/ClimberIOSim.java

@@ -17,15 +17,15 @@ public class ClimberIOSim implements ClimberIO {
 
 	public ClimberIOSim() {
 		// Lol I'm not gona tune the sim
-		climberSim = new DCMotorSim(DCMotor.getNEO(2), 1, 1);
+		climberSim = new DCMotorSim(DCMotor.getNEO(Constants.Climber.NUM_MOTORS), Constants.Climber.GEARING, Constants.Climber.JKG_M_SQUARED);
 		controller = new PIDController(0, 0, 0);
 		climberSetpoint = 0;
 		appliedVolts = 0;
 	}
 
 	public void updateInput(ClimberIOInputs inputs) {
-		inputs.leftClimberSpeed = climberSim.getAngularVelocityRPM() / 60;
-		inputs.rightClimberSpeed = climberSim.getAngularVelocityRPM() / 60;
+		inputs.leftClimberSpeed = climberSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF;
+		inputs.rightClimberSpeed = climberSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF;
 		inputs.leftClimberPosition = climberSim.getAngularPositionRotations();
 		inputs.rightClimberPosition = climberSim.getAngularPositionRotations();
 		inputs.leftClimberSetpoint = climberSetpoint;

src/main/java/frc/robot/subsystems/climbers/ClimberIOSparkMax.java

@@ -46,8 +46,8 @@ public ClimberIOSparkMax() {
 
 		climberController = new PIDController(0, 0, 0);
 
-		leftFilter = LinearFilter.movingAverage(5);
-		rightFilter = LinearFilter.movingAverage(5);
+		leftFilter = LinearFilter.movingAverage(Constants.Climber.CURRENT_FILTER_TAPS);
+		rightFilter = LinearFilter.movingAverage(Constants.Climber.CURRENT_FILTER_TAPS);
 
 		leftClimberSetpoint = 0;
 		rightClimberSetpoint = 0;
@@ -64,8 +64,8 @@ public void updateInputs(ClimberIOInputs inputs) {
 		inputs.rightClimberPosition = rightClimberEncoder.getPosition();
 		inputs.leftClimberSetpoint = leftClimberSetpoint;
 		inputs.rightClimberSetpoint = rightClimberSetpoint;
-		inputs.leftClimberSpeed = leftClimberEncoder.getVelocity() / 60;
-		inputs.rightClimberSpeed = rightClimberEncoder.getVelocity() / 60;
+		inputs.leftClimberSpeed = leftClimberEncoder.getVelocity() / Constants.RPM_TO_RPS_CF;
+		inputs.rightClimberSpeed = rightClimberEncoder.getVelocity() / Constants.RPM_TO_RPS_CF;
 	}
 
 	public void updateOutputs(ClimberIOOutputs outputs) {
@@ -108,8 +108,8 @@ public void setSetpoints(double leftSetpoint, double rightSetpoint) {
 	}
 
 	public void zeroClimbers() {
-		double leftZeroingSpeed = -0.25;
-		double rightZeroingSpeed = -0.25;
+		double leftZeroingSpeed = Constants.Climber.ZEROING_SPEED;
+		double rightZeroingSpeed = Constants.Climber.ZEROING_SPEED;
 
 		// Not voltage sensing
 		if (
@@ -118,7 +118,7 @@ public void zeroClimbers() {
 			leftClimberZeroed
 		) {
 			if (!leftClimberZeroed) leftClimberMotor.getEncoder().setPosition(0);
-			leftClimberSetpoint = 10;
+			leftClimberSetpoint = Constants.Climber.IDLE;
 			leftZeroingSpeed = climberController.calculate(
 				leftClimberEncoder.getPosition(),
 				leftClimberSetpoint
@@ -132,7 +132,7 @@ public void zeroClimbers() {
 			rightClimberZeroed
 		) {
 			if (!rightClimberZeroed) rightClimberMotor.getEncoder().setPosition(0);
-			rightClimberSetpoint = 10;
+			rightClimberSetpoint = Constants.Climber.IDLE;
 			rightZeroingSpeed = climberController.calculate(
 				rightClimberEncoder.getPosition(),
 				rightClimberSetpoint

src/main/java/frc/robot/subsystems/climbers/ClimberStates.java

@@ -1,10 +1,11 @@
 package frc.robot.subsystems.climbers;
 
+import frc.robot.Constants;
 import frc.robot.subsystems.SubsystemStates;
 
 public enum ClimberStates implements SubsystemStates {
-	OFF(10, 10, "Off"),
-	CLIMBING(150, 150, "Climbing");
+	OFF(Constants.Climber.IDLE, Constants.Climber.IDLE, "Off"),
+	CLIMBING(Constants.Climber.CLIMBING, Constants.Climber.CLIMBING, "Climbing");
 
 	private double leftSetpoint;
 	private double rightSetpoint;