Initial Climb Subsystem

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

@@ -1,3 +1,151 @@
 package frc.robot.constants;
 
-public class ClimbConstants {}
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
+import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+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;
+
+public class ClimbConstants {
+  public static int kWheelFxId = 0;
+  public static int kArmFxId = 0;
+
+  public static final AngularVelocity kWheelCloseEnough = DegreesPerSecond.of(5.0);
+  public static final Angle kArmCloseEnough = Degrees.of(5.0);
+  public static final Angle kArmMaxFwd = Degrees.of(100);
+  public static final Angle kArmMaxRev = Degrees.of(-100);
+  public static final Angle kArmZeroTicks = Degrees.of(1530);
+
+  public static TalonFXConfiguration getWheelFXConfig() {
+    TalonFXConfiguration wheelFxConfig = new TalonFXConfiguration();
+
+    CurrentLimitsConfigs current =
+        new CurrentLimitsConfigs()
+            .withStatorCurrentLimit(10)
+            .withStatorCurrentLimitEnable(false)
+            .withStatorCurrentLimit(20)
+            .withSupplyCurrentLimit(10)
+            .withSupplyCurrentLowerLimit(8)
+            .withSupplyCurrentLowerTime(0.02)
+            .withSupplyCurrentLimitEnable(true);
+    wheelFxConfig.CurrentLimits = current;
+
+    HardwareLimitSwitchConfigs hwLimit =
+        new HardwareLimitSwitchConfigs()
+            .withForwardLimitAutosetPositionEnable(false)
+            .withForwardLimitEnable(false)
+            .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
+            .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
+            .withReverseLimitAutosetPositionEnable(false)
+            .withReverseLimitEnable(false)
+            .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
+            .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
+    wheelFxConfig.HardwareLimitSwitch = hwLimit;
+
+    Slot0Configs slot0 =
+        new Slot0Configs()
+            .withKP(0)
+            .withKI(0)
+            .withKD(0)
+            .withGravityType(GravityTypeValue.Elevator_Static)
+            .withKG(0)
+            .withKS(0)
+            .withKV(0)
+            .withKA(0);
+    wheelFxConfig.Slot0 = slot0;
+
+    MotionMagicConfigs motionMagic =
+        new MotionMagicConfigs()
+            .withMotionMagicAcceleration(0)
+            .withMotionMagicCruiseVelocity(0)
+            .withMotionMagicExpo_kA(0)
+            .withMotionMagicExpo_kV(0)
+            .withMotionMagicJerk(0);
+    wheelFxConfig.MotionMagic = motionMagic;
+
+    MotorOutputConfigs motorOut =
+        new MotorOutputConfigs()
+            .withDutyCycleNeutralDeadband(0.01)
+            .withNeutralMode(NeutralModeValue.Coast);
+    wheelFxConfig.MotorOutput = motorOut;
+
+    return wheelFxConfig;
+  }
+
+  public static TalonFXConfiguration getArmFxConfig() {
+    TalonFXConfiguration armFxConfig = new TalonFXConfiguration();
+
+    CurrentLimitsConfigs current =
+        new CurrentLimitsConfigs()
+            .withStatorCurrentLimit(10)
+            .withStatorCurrentLimitEnable(false)
+            .withStatorCurrentLimit(20)
+            .withSupplyCurrentLimit(10)
+            .withSupplyCurrentLowerLimit(8)
+            .withSupplyCurrentLowerTime(0.02)
+            .withSupplyCurrentLimitEnable(true);
+    armFxConfig.CurrentLimits = current;
+
+    HardwareLimitSwitchConfigs hwLimit =
+        new HardwareLimitSwitchConfigs()
+            .withForwardLimitAutosetPositionEnable(false)
+            .withForwardLimitEnable(false)
+            .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
+            .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
+            .withReverseLimitAutosetPositionEnable(false)
+            .withReverseLimitEnable(false)
+            .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
+            .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
+    armFxConfig.HardwareLimitSwitch = hwLimit;
+
+    SoftwareLimitSwitchConfigs swLimit =
+        new SoftwareLimitSwitchConfigs()
+            .withForwardSoftLimitEnable(true)
+            .withForwardSoftLimitThreshold(kArmMaxFwd)
+            .withReverseSoftLimitEnable(true)
+            .withReverseSoftLimitThreshold(kArmMaxRev);
+    armFxConfig.SoftwareLimitSwitch = swLimit;
+
+    Slot0Configs slot0 =
+        new Slot0Configs()
+            .withKP(0)
+            .withKI(0)
+            .withKD(0)
+            .withGravityType(GravityTypeValue.Elevator_Static)
+            .withKG(0)
+            .withKS(0)
+            .withKV(0)
+            .withKA(0);
+    armFxConfig.Slot0 = slot0;
+
+    MotionMagicConfigs motionMagic =
+        new MotionMagicConfigs()
+            .withMotionMagicAcceleration(0)
+            .withMotionMagicCruiseVelocity(0)
+            .withMotionMagicExpo_kA(0)
+            .withMotionMagicExpo_kV(0)
+            .withMotionMagicJerk(0);
+    armFxConfig.MotionMagic = motionMagic;
+
+    MotorOutputConfigs motorOut =
+        new MotorOutputConfigs()
+            .withDutyCycleNeutralDeadband(0.01)
+            .withNeutralMode(NeutralModeValue.Coast);
+    armFxConfig.MotorOutput = motorOut;
+
+    return armFxConfig;
+  }
+}

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

@@ -0,0 +1,23 @@
+package frc.robot.subsystems.climb;
+
+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;
+
+public interface ClimbArmIO {
+
+  @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() {}
+
+  public default void registerWith(TelemetryService telemetryService) {}
+}

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

@@ -0,0 +1,65 @@
+package frc.robot.subsystems.climb;
+
+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> curPosition;
+
+  public ClimbArmIOFX() {
+    logger = LoggerFactory.getLogger(this.getClass());
+    talonFx = new TalonFX(ClimbConstants.kArmFxId);
+    absSensorInitial =
+        talonFx.getPosition().getValue(); // relative encoder starts up as absolute position offset
+
+    configurator = talonFx.getConfigurator();
+    configurator.apply(new TalonFXConfiguration()); // Factory default motor controller
+    configurator.apply(ClimbConstants.getArmFxConfig());
+
+    curPosition = talonFx.getPosition();
+  }
+
+  @Override
+  public void setPosition(Angle position) {
+    setpoint = position.plus(relSetpointOffset);
+
+    logger.info("Setting position to {} rotations", setpoint.in(Rotations));
+
+    talonFx.setControl(positionRequest.withPosition(setpoint));
+  }
+
+  @Override
+  public void updateInputs(ClimbArmIOInputs inputs) {
+    inputs.position = curPosition.refresh().getValue();
+  }
+
+  @Override
+  public void zero() {
+    relSetpointOffset = ExampleConstants.kZeroTicks;
+    logger.info(
+        "Abs: {}, Zero Pos: {}, Offset: {}",
+        absSensorInitial,
+        ExampleConstants.kZeroTicks,
+        absSensorInitial.minus(ExampleConstants.kZeroTicks));
+  }
+}

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

@@ -1,3 +1,89 @@
 package frc.robot.subsystems.climb;
 
-public class ClimbSubsystem {}
+import static edu.wpi.first.units.Units.Rotations;
+
+import edu.wpi.first.units.measure.Angle;
+import frc.robot.constants.ClimbConstants;
+import frc.robot.standards.ClosedLoopPosSubsystem;
+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 ClimbArmIO io;
+  private final ClimbWheelIO wheelIo;
+
+  private final ClimbArmIOInputsAutoLogged inputs = new ClimbArmIOInputsAutoLogged();
+
+  private Angle setpoint = Rotations.of(0.0);
+  private ClimbState curState = ClimbState.INIT;
+
+  public ClimbSubsystem(ClimbArmIO io, ClimbWheelIO wheelIo) {
+    this.io = io;
+    this.wheelIo = wheelIo;
+
+    zero();
+  }
+
+  @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);
+  }
+
+  @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

@@ -0,0 +1,10 @@
+package frc.robot.subsystems.climb;
+
+import org.strykeforce.telemetry.TelemetryService;
+
+public interface ClimbWheelIO {
+
+  public default void setPercent(double pct) {}
+
+  public default void registerWith(TelemetryService telemetryService) {}
+}

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

@@ -0,0 +1,37 @@
+package frc.robot.subsystems.climb;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.hardware.TalonFX;
+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;
+
+  public ClimbWheelIOFX() {
+    logger = LoggerFactory.getLogger(this.getClass());
+    talonFx = new TalonFX(ClimbConstants.kWheelFxId);
+
+    configurator = talonFx.getConfigurator();
+    configurator.apply(new TalonFXConfiguration()); // Factory default motor controller
+    configurator.apply(ClimbConstants.getWheelFXConfig());
+  }
+
+  @Override
+  public void setPercent(double pct) {
+    logger.info("Setting percent to {}", pct);
+
+    talonFx.set(pct);
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    telemetryService.register(talonFx, true);
+  }
+}