RewriteClimber

src/main/java/frc/robot/Manager/Manager.java

@@ -28,7 +28,7 @@ public static Manager getInstance() {
 
 	public Manager() {
 		super("Manager", ManagerStates.IDLE);
-		climber = new Climber();
+		climber = Climber.getInstance();
 		algaeCoraler = AlgaeCoraler.getInstance();
 		drive = Drive.getInstance();
 

src/main/java/frc/robot/Subsystems/Climber/Climber.java

@@ -1,19 +1,26 @@
 package frc.robot.Subsystems.Climber;
 
 import static frc.robot.GlobalConstants.*;
-import static frc.robot.GlobalConstants.Controllers.DRIVER_CONTROLLER;
-import static frc.robot.GlobalConstants.Controllers.OPERATOR_CONTROLLER;
 import static frc.robot.Subsystems.Climber.ClimberConstants.*;
 
 import org.littletonrobotics.junction.Logger;
 import org.team7525.subsystem.Subsystem;
 
 public class Climber extends Subsystem<ClimberStates> {
 
+	private static Climber instance;
+
 	private ClimberIO io;
 	private ClimberIOInputsAutoLogged inputs;
 
-	public Climber() {
+	public static Climber getInstance() {
+		if (instance == null) {
+			instance = new Climber();
+		}
+		return instance;
+	}
+
+	private Climber() {
 		super("Climber", ClimberStates.IDLE);
 		this.io = switch (ROBOT_MODE) {
 			case REAL -> new ClimberIOReal();
@@ -26,22 +33,9 @@ public Climber() {
 
 	@Override
 	public void runState() {
+		io.setSpeed(getState().getSpeed());
 		io.updateInputs(inputs);
 
-		if (
-			Math.abs(DRIVER_CONTROLLER.getRightTriggerAxis()) > 0.5 ||
-			Math.abs(OPERATOR_CONTROLLER.getRightTriggerAxis()) > 0.5
-		) {
-			io.setSpeed(ClimberStates.IN.getSpeed());
-		} else if (
-			Math.abs(DRIVER_CONTROLLER.getLeftTriggerAxis()) > 0.5 ||
-			Math.abs(OPERATOR_CONTROLLER.getLeftTriggerAxis()) > 0.5
-		) {
-			io.setSpeed(ClimberStates.OUT.getSpeed());
-		} else {
-			io.setSpeed(ClimberStates.IDLE.getSpeed());
-		}
-
 		Logger.processInputs(SUBSYSTEM_NAME, inputs);
 		Logger.recordOutput(SUBSYSTEM_NAME + "/State", getState().getStateString());
 	}

src/main/java/frc/robot/Subsystems/Climber/ClimberConstants.java

@@ -13,8 +13,6 @@ public final class ClimberConstants {
 	public static final double IN_SPEED = -0.65;
 	public static final double OUT_SPEED = 0.65;
 
-	// TODO: set values ^
-
 	public static class Sim {
 
 		public static final double MOTOR_GEARING = 25;

src/main/java/frc/robot/Subsystems/Climber/ClimberIO.java

@@ -7,6 +7,7 @@ public interface ClimberIO {
 	public static class ClimberIOInputs {
 
 		public double speed;
+		public double speedSetpoint;
 	}
 
 	public void updateInputs(ClimberIOInputs input);

src/main/java/frc/robot/Subsystems/Climber/ClimberIOReal.java

@@ -9,6 +9,8 @@ public class ClimberIOReal implements ClimberIO {
 
 	private SparkMax motor;
 
+	private double speedSetpoint;
+
 	public ClimberIOReal() {
 		motor = new SparkMax(CLIMBER_CANID, MotorType.kBrushless);
 		motor.getEncoder().setPosition(0);
@@ -17,10 +19,12 @@ public ClimberIOReal() {
 	@Override
 	public void updateInputs(ClimberIOInputs inputs) {
 		inputs.speed = motor.getEncoder().getVelocity() / 60;
+		inputs.speedSetpoint = speedSetpoint;
 	}
 
 	@Override
 	public void setSpeed(double speed) {
+		this.speedSetpoint = speed;
 		motor.set(speed);
 	}
 }

src/main/java/frc/robot/Subsystems/Climber/ClimberStates.java

@@ -22,7 +22,7 @@ public String getStateString() {
 		return stateString;
 	}
 
-	protected double getSpeed() {
+	public double getSpeed() {
 		return speed;
 	}
 }