stubbed out climb subsystem

Author: imroot07

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

@@ -16,7 +16,6 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
 import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
-
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 

src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java

@@ -2,19 +2,19 @@
 
 import static edu.wpi.first.units.Units.Rotations;
 
+import edu.wpi.first.units.measure.Angle;
 import org.littletonrobotics.junction.AutoLog;
 import org.strykeforce.telemetry.TelemetryService;
 
-import edu.wpi.first.units.measure.Angle;
-
 public interface ClimbArmIO {
-    
-    @AutoLog static class ClimbArmIOInputs {
-        public Angle position = Rotations.of(0.0);
-    }
+
+  @AutoLog
+  static class ClimbArmIOInputs {
+    public Angle position = Rotations.of(0.0);
+  }
 
   public default void setPosition(Angle position) {}
-  
+
   public default void updateInputs(ClimbArmIOInputs inputs) {}
 
   public default void zero() {}

src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java

@@ -1,31 +1,30 @@
 package frc.robot.subsystems.climb;
 
-import static edu.wpi.first.units.Units.Degrees;
-
-import org.slf4j.Logger;
-import org.slf4j.LoggerFactory;
+import static edu.wpi.first.units.Units.Rotations;
 
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfigurator;
 import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
-
 import edu.wpi.first.units.measure.Angle;
 import frc.robot.constants.ClimbConstants;
 import frc.robot.constants.ExampleConstants;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 public class ClimbArmIOFX implements ClimbArmIO {
   private Logger logger;
   private TalonFX talonFx;
-  
+
   private final Angle absSensorInitial;
   private Angle relSetpointOffset;
   private Angle setpoint;
 
   TalonFXConfigurator configurator;
-  private MotionMagicDutyCycle positionRequest = new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
-  StatusSignal<Angle> currPosition;
+  private MotionMagicDutyCycle positionRequest =
+      new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
+  StatusSignal<Angle> curPosition;
 
   public ClimbArmIOFX() {
     logger = LoggerFactory.getLogger(this.getClass());
@@ -37,21 +36,21 @@ public ClimbArmIOFX() {
     configurator.apply(new TalonFXConfiguration()); // Factory default motor controller
     configurator.apply(ClimbConstants.getArmFxConfig());
 
-    currPosition = talonFx.getPosition();
+    curPosition = talonFx.getPosition();
   }
 
   @Override
   public void setPosition(Angle position) {
     setpoint = position.plus(relSetpointOffset);
 
-    logger.info("Setting position to {} degrees", setpoint.in(Degrees));
+    logger.info("Setting position to {} rotations", setpoint.in(Rotations));
 
     talonFx.setControl(positionRequest.withPosition(setpoint));
   }
 
   @Override
   public void updateInputs(ClimbArmIOInputs inputs) {
-      inputs.position = currPosition.refresh().getValue();
+    inputs.position = curPosition.refresh().getValue();
   }
 
   @Override

src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java

@@ -2,39 +2,88 @@
 
 import static edu.wpi.first.units.Units.Rotations;
 
-import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
-
 import edu.wpi.first.units.measure.Angle;
+import frc.robot.constants.ClimbConstants;
 import frc.robot.standards.ClosedLoopPosSubsystem;
-import frc.robot.subsystems.climb.ClimbArmIO.ClimbArmIOInputs;
-import frc.robot.subsystems.example.ExampleIOInputsAutoLogged;
-import frc.robot.subsystems.example.ExampleSubsystem.ExampleState;
+import java.util.Set;
+import org.littletonrobotics.junction.Logger;
+import org.strykeforce.telemetry.TelemetryService;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
 
 public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem {
-    private final ClimbWheelIO wheelIo;
-    private final ClimbArmIO armIo;
+  private final ClimbArmIO io;
+  private final ClimbWheelIO wheelIo;
 
-    private final ClimbWheelIOInputsAutoLogged wheelInputs = new ExampleIOInputsAutoLogged();
-    private final ClimbArmIOInputsAutoLogged armInputs = new ExampleIOInputsAutoLogged();
+  private final ClimbArmIOInputsAutoLogged inputs = new ClimbArmIOInputsAutoLogged();
 
-    private Angle setpoint = Rotations.of(0.0);
-    private ClimbState curState = ClimbState.INIT;
+  private Angle setpoint = Rotations.of(0.0);
+  private ClimbState curState = ClimbState.INIT;
 
-    public ClimbSubsystem(ClimbWheelIO wheelIo, ClimbArmIO armIo) {
-        this.wheelIo = wheelIo;
-        this.armIo=armIo;
+  public ClimbSubsystem(ClimbArmIO io, ClimbWheelIO wheelIo) {
+    this.io = io;
+    this.wheelIo = wheelIo;
 
-        zero();
-    }
+    zero();
+  }
 
-    @Override
-    public void zero() {
-        armIo.zero();
-        curState = ClimbState.ZEROED;
-    }
+  @Override
+  public Angle getPosition() {
+    return inputs.position;
+  }
+
+  @Override
+  public void setPosition(Angle position) {
+    io.setPosition(position);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return setpoint.minus(inputs.position).abs(Rotations)
+        <= ClimbConstants.kArmCloseEnough.in(Rotations);
+  }
 
-    public enum ClimbState {
-        INIT,
-        ZEROED
+  @Override
+  public void zero() {
+    io.zero();
+
+    curState = ClimbState.ZEROED;
+  }
+
+  @Override
+  public void periodic() {
+    // Read Inputs
+    io.updateInputs(inputs);
+
+    // State Machine
+    switch (curState) {
+      case INIT:
+        break;
+      case ZEROED:
+        break;
+      default:
+        break;
     }
+
+    // Log Outputs
+    Logger.recordOutput("Coral/curState", curState.ordinal());
+    Logger.recordOutput("Coral/setpoint", setpoint.in(Rotations));
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    super.registerWith(telemetryService);
+    wheelIo.registerWith(telemetryService);
+    io.registerWith(telemetryService);
+  }
+
+  @Override
+  public Set<Measure> getMeasures() {
+    return Set.of();
+  }
+
+  public enum ClimbState {
+    INIT,
+    ZEROED
+  }
 }

src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java

@@ -1,21 +1,10 @@
 package frc.robot.subsystems.climb;
 
-import static edu.wpi.first.units.Units.RotationsPerSecond;
-
-import org.littletonrobotics.junction.AutoLog;
 import org.strykeforce.telemetry.TelemetryService;
 
-import edu.wpi.first.units.measure.AngularVelocity;
-
 public interface ClimbWheelIO {
-    
-    @AutoLog static class ClimbWheelIOInputs {
-        public AngularVelocity velocity = RotationsPerSecond.of(0.0);
-    }
 
-  public default void setVelocity(AngularVelocity velocity) {}
-  
-  public default void updateInputs(ClimbWheelIOInputs inputs) {}
+  public default void setPercent(double pct) {}
 
   public default void registerWith(TelemetryService telemetryService) {}
 }

src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java

@@ -1,27 +1,18 @@
 package frc.robot.subsystems.climb;
 
-import static edu.wpi.first.units.Units.DegreesPerSecond;
-
-import org.slf4j.Logger;
-import org.slf4j.LoggerFactory;
-import org.strykeforce.telemetry.TelemetryService;
-
-import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfigurator;
-import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
-
-import edu.wpi.first.units.measure.AngularVelocity;
 import frc.robot.constants.ClimbConstants;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
 
 public class ClimbWheelIOFX implements ClimbWheelIO {
   private Logger logger;
   private TalonFX talonFx;
 
   TalonFXConfigurator configurator;
-  private MotionMagicVelocityDutyCycle velocityRequest = new MotionMagicVelocityDutyCycle(0).withEnableFOC(false).withSlot(0);
-  StatusSignal<AngularVelocity> curVelocity;
 
   public ClimbWheelIOFX() {
     logger = LoggerFactory.getLogger(this.getClass());
@@ -30,24 +21,17 @@ public ClimbWheelIOFX() {
     configurator = talonFx.getConfigurator();
     configurator.apply(new TalonFXConfiguration()); // Factory default motor controller
     configurator.apply(ClimbConstants.getWheelFXConfig());
-
-    curVelocity = talonFx.getVelocity();
   }
 
   @Override
-  public void setVelocity(AngularVelocity velocity) {
-    logger.info("Setting velocity to {} degrees per second", velocity.in(DegreesPerSecond));
+  public void setPercent(double pct) {
+    logger.info("Setting percent to {}", pct);
 
-      talonFx.setControl(velocityRequest.withVelocity(velocity));
-  }
-
-  @Override
-  public void updateInputs(ClimbWheelIOInputs inputs) {
-      inputs.velocity = curVelocity.refresh().getValue();
+    talonFx.set(pct);
   }
 
   @Override
   public void registerWith(TelemetryService telemetryService) {
-      telemetryService.register(talonFx, true);
+    telemetryService.register(talonFx, true);
   }
 }

src/main/java/frc/robot/subsystems/example/ExampleIOFX.java

@@ -27,8 +27,8 @@ public class ExampleIOFX implements ExampleIO {
   TalonFXConfigurator configurator;
   private MotionMagicDutyCycle positionRequest =
       new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
-  StatusSignal<Angle> currPosition;
-  StatusSignal<AngularVelocity> currVelocity;
+  StatusSignal<Angle> curPosition;
+  StatusSignal<AngularVelocity> curVelocity;
 
   public ExampleIOFX() {
     logger = LoggerFactory.getLogger(this.getClass());
@@ -42,8 +42,8 @@ public ExampleIOFX() {
     configurator.apply(ExampleConstants.getFXConfig());
 
     // Attach status signals
-    currPosition = talonFx.getPosition();
-    currVelocity = talonFx.getVelocity();
+    curPosition = talonFx.getPosition();
+    curVelocity = talonFx.getVelocity();
   }
 
   @Override
@@ -64,9 +64,9 @@ public void setPosition(Angle position) {
 
   @Override
   public void updateInputs(ExampleIOInputs inputs) {
-    BaseStatusSignal.refreshAll(currVelocity, currPosition);
-    inputs.velocity = currVelocity.getValue();
-    inputs.position = currPosition.getValue().minus(relSetpointOffset);
+    BaseStatusSignal.refreshAll(curVelocity, curPosition);
+    inputs.velocity = curVelocity.getValue();
+    inputs.position = curPosition.getValue().minus(relSetpointOffset);
   }
 
   @Override