diff --git a/src/main/java/frc/robot/Manager/Manager.java b/src/main/java/frc/robot/Manager/Manager.java index b4720a9..ec961f1 100644 --- a/src/main/java/frc/robot/Manager/Manager.java +++ b/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(); diff --git a/src/main/java/frc/robot/Subsystems/Climber/Climber.java b/src/main/java/frc/robot/Subsystems/Climber/Climber.java index beb634d..f5f41be 100644 --- a/src/main/java/frc/robot/Subsystems/Climber/Climber.java +++ b/src/main/java/frc/robot/Subsystems/Climber/Climber.java @@ -1,8 +1,6 @@ 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; @@ -10,10 +8,19 @@ public class Climber extends Subsystem { + 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()); } diff --git a/src/main/java/frc/robot/Subsystems/Climber/ClimberConstants.java b/src/main/java/frc/robot/Subsystems/Climber/ClimberConstants.java index b450e37..942a79f 100644 --- a/src/main/java/frc/robot/Subsystems/Climber/ClimberConstants.java +++ b/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; diff --git a/src/main/java/frc/robot/Subsystems/Climber/ClimberIO.java b/src/main/java/frc/robot/Subsystems/Climber/ClimberIO.java index 244bcf9..741858a 100644 --- a/src/main/java/frc/robot/Subsystems/Climber/ClimberIO.java +++ b/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); diff --git a/src/main/java/frc/robot/Subsystems/Climber/ClimberIOReal.java b/src/main/java/frc/robot/Subsystems/Climber/ClimberIOReal.java index 1208bea..5c2d0e9 100644 --- a/src/main/java/frc/robot/Subsystems/Climber/ClimberIOReal.java +++ b/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); } } diff --git a/src/main/java/frc/robot/Subsystems/Climber/ClimberStates.java b/src/main/java/frc/robot/Subsystems/Climber/ClimberStates.java index 10b2c90..8603a84 100644 --- a/src/main/java/frc/robot/Subsystems/Climber/ClimberStates.java +++ b/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; } }