First pass at climber subsystem

src/main/java/frc/robot/Robot.java

@@ -31,6 +31,9 @@
 import frc.robot.Superstructure.SuperState;
 import frc.robot.components.rollers.RollerIO;
 import frc.robot.components.rollers.RollerIOSim;
+import frc.robot.subsystems.climber.ClimberIO;
+import frc.robot.subsystems.climber.ClimberSubsystem;
+import frc.robot.subsystems.climber.EmptyClimberSubsystem;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.indexer.LindexerSubsystem;
 import frc.robot.subsystems.indexer.SpindexerSubsystem;
@@ -166,6 +169,7 @@ public enum RobotEdition {
   // swervesubsystem decides on its own whether or not to use alpha or comp swerve constants
   private final SwerveSubsystem swerve = new SwerveSubsystem(canivore);
   private final LEDSubsystem leds;
+  private final ClimberSubsystem climber;
 
   // climber only exists for the comp bot - this is accounted for later
 
@@ -323,10 +327,12 @@ public Robot() {
                     ? new HoodIO(HoodIO.getCompHood(), canivore, 11)
                     : new HoodIOSim(
                         canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11));
-
-        // TODO climber
         break;
     }
+    climber =
+        ROBOT_EDITION == RobotEdition.ALPHA
+            ? new EmptyClimberSubsystem(canivore)
+            : new ClimberSubsystem(new ClimberIO(canivore));
     // now that we've assigned the correct subsystems based on robot edition, we can pass them into
     // the superstructure
     superstructure = new Superstructure(swerve, indexer, intake, shooter, driver, operator);
@@ -337,9 +343,6 @@ public Robot() {
                 () ->
                     Superstructure.getState() == SuperState.SPIN_UP_SCORE
                         || Superstructure.getState() == SuperState.SCORE);
-    // if this is alpha, we won't have assigned a climber yet
-    // this creates a placeholder "no-operation" climber that will just not do anything, but is not
-    // null (and we need it to be not null)
 
     DriverStation.silenceJoystickConnectionWarning(true);
     SignalLogger.enableAutoLogging(false);
@@ -525,7 +528,10 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
 
     // TODO: autoaim (comp)
     // autoAimReq.and(() -> ROBOT_EDITION == RobotEdition.COMP).whileTrue();
-    // TODO add binding for climb
+
+    // TODO ACTUAL BUTTON BINDING FOR CLIMBER
+    driver.x().onTrue(climber.extendClimber());
+    driver.y().onTrue(climber.retractClimber());
 
     // current zero shooter hood
     driver.b().whileTrue(shooter.runCurrentZeroing());

src/main/java/frc/robot/subsystems/climber/ClimberIO.java

@@ -0,0 +1,126 @@
+package frc.robot.subsystems.climber;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Temperature;
+import edu.wpi.first.units.measure.Voltage;
+import org.littletonrobotics.junction.AutoLog;
+
+public class ClimberIO {
+
+  @AutoLog
+  public static class ClimberIOInputs {
+    public Rotation2d motorPositionRotations = new Rotation2d();
+    public double motorVelocityMetersPerSec = 0.0;
+    public double motorStatorCurrentAmps = 0.0;
+    public double motorSupplyCurrentAmps = 0.0;
+    public double motorVoltage = 0.0;
+    public double motorTempC = 0.0;
+  }
+
+  protected final TalonFX climberMotor;
+
+  private final StatusSignal<Angle> motorPositionRotations;
+  private final StatusSignal<AngularVelocity> angularVelocityRotsPerSec;
+  private final StatusSignal<Current> statorCurrentAmps;
+  private final StatusSignal<Current> supplyCurrentAmps;
+  private final StatusSignal<Voltage> motorVoltage;
+  private final StatusSignal<Temperature> motorTemp;
+
+  private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
+  private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true);
+  private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true);
+
+  private double climberSetpoint = 0.0;
+
+  public ClimberIO(CANBus canBus) {
+    // todo: set correct motor ID
+    climberMotor = new TalonFX(30, canBus);
+    climberMotor.getConfigurator().apply(ClimberIO.getClimberConfiguration());
+
+    angularVelocityRotsPerSec = climberMotor.getVelocity();
+    supplyCurrentAmps = climberMotor.getSupplyCurrent();
+    motorVoltage = climberMotor.getMotorVoltage();
+    statorCurrentAmps = climberMotor.getStatorCurrent();
+    motorTemp = climberMotor.getDeviceTemp();
+    motorPositionRotations = climberMotor.getPosition();
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        50.0,
+        angularVelocityRotsPerSec,
+        supplyCurrentAmps,
+        motorVoltage,
+        statorCurrentAmps,
+        motorTemp,
+        motorPositionRotations);
+    climberMotor.optimizeBusUtilization();
+  }
+
+  public static TalonFXConfiguration getClimberConfiguration() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+    // todo: find and make climber gear ratio variable
+    config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO;
+
+    // todo: tune
+    config.Slot0.kS = 0.0;
+    config.Slot0.kG = 0.0;
+    config.Slot0.kV = 0.0;
+    config.Slot0.kP = 0.0;
+    config.Slot0.kD = 0.0;
+
+    // todo: find actual current limits
+    config.CurrentLimits.StatorCurrentLimit = 50.00;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.CurrentLimits.SupplyCurrentLimit = 40.00;
+
+    return config;
+  }
+
+  public void setClimberPosition(double climberPosition) {
+    climberSetpoint = climberPosition;
+    climberMotor.setControl(positionVoltage.withPosition(climberSetpoint));
+  }
+
+  public void setClimberVoltage(double climberVoltage) {
+    climberMotor.setControl(voltageOut.withOutput(climberVoltage));
+  }
+
+  public void setClimberVelocity(double climberVelocity) {
+    climberMotor.setControl(velocityVoltage.withVelocity(climberVelocity));
+  }
+
+  public void updateInputs(ClimberIOInputs inputs) {
+    BaseStatusSignal.refreshAll(
+        motorPositionRotations,
+        angularVelocityRotsPerSec,
+        statorCurrentAmps,
+        supplyCurrentAmps,
+        motorVoltage,
+        motorTemp);
+
+    inputs.motorPositionRotations =
+        Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble());
+    inputs.motorVoltage = motorVoltage.getValueAsDouble();
+    inputs.motorTempC = motorTemp.getValueAsDouble();
+    inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble();
+    inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble();
+    inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble();
+  }
+}

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

@@ -0,0 +1,23 @@
+package frc.robot.subsystems.climber;
+
+// unfinished
+
+// public class ClimberIOSim extends ClimberIO {
+//   TalonFXSimState climberMotorSim;
+
+//   private final DCMotorSim climberPhysicsSim =
+//       new DCMotorSim(
+//           LinearSystemId.createDCMotorSystem(
+//               DCMotor.getKrakenX44Foc(1), 0.01, ClimberSubsystem.GEAR_RATIO),
+//           DCMotor.getKrakenX44Foc(1));
+
+//   private final double simLoopPeriod = 0.002;
+//   private Notifier simNotifier = null;
+//   private double lastSimTime = 0.0;
+
+//   public ClimberIOSim(CANBus canbus) {
+//     super(ClimberIO.getClimberConfiguration(), canbus);
+//     climberMotorSim = climberMotor.getSimState();
+//     climberMotorSim.setMotorType(MotorType.KrakenX60);
+//   }
+// }

src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java

@@ -0,0 +1,42 @@
+package frc.robot.subsystems.climber;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class ClimberSubsystem extends SubsystemBase {
+  // todo: find actual constants
+  public static double GEAR_RATIO = (45.0 / 1.0);
+  public static double MAX_EXTENSION_METERS = 0.2413;
+  public static double MAX_ACCELERATION = 10.0;
+  public static double MAX_VELOCITY = 2.0;
+
+  ClimberIO climberIO;
+  ClimberIOInputsAutoLogged climberInputs = new ClimberIOInputsAutoLogged();
+
+  @Override
+  public void periodic() {
+    climberIO.updateInputs(climberInputs);
+    Logger.processInputs("Climber", climberInputs);
+  }
+
+  // member variables here?
+
+  public ClimberSubsystem(ClimberIO climberIO) {
+    this.climberIO = climberIO;
+  }
+
+  public Command extendClimber() {
+    return this.run(
+        () -> {
+          climberIO.setClimberPosition(MAX_EXTENSION_METERS);
+        });
+  }
+
+  public Command retractClimber() {
+    return this.run(
+        () -> {
+          climberIO.setClimberPosition(0.0);
+        });
+  }
+}

src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java

@@ -0,0 +1,24 @@
+package frc.robot.subsystems.climber;
+
+import com.ctre.phoenix6.CANBus;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class EmptyClimberSubsystem extends ClimberSubsystem {
+
+  @Override
+  public void periodic() {}
+
+  public EmptyClimberSubsystem(CANBus canbus) {
+    super(new ClimberIO(canbus));
+  }
+
+  @Override
+  public Command extendClimber() {
+    return this.idle();
+  }
+
+  @Override
+  public Command retractClimber() {
+    return this.idle();
+  }
+}