Climber fixes

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

@@ -10,19 +10,19 @@
 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;
+import org.littletonrobotics.junction.AutoLogOutput;
 
 public class ClimberIO {
 
   @AutoLog
   public static class ClimberIOInputs {
-    public Rotation2d motorPositionRotations = new Rotation2d();
+    public double motorPositionMeters = 0.0;
     public double motorVelocityMetersPerSec = 0.0;
     public double motorStatorCurrentAmps = 0.0;
     public double motorSupplyCurrentAmps = 0.0;
@@ -32,8 +32,9 @@ public static class ClimberIOInputs {
 
   protected final TalonFX climberMotor;
 
-  private final StatusSignal<Angle> motorPositionRotations;
-  private final StatusSignal<AngularVelocity> angularVelocityRotsPerSec;
+  // Rotation -> linear conversion happens in sensor to mech ratio
+  private final StatusSignal<Angle> motorPositionMeters;
+  private final StatusSignal<AngularVelocity> velocityMetersPerSec;
   private final StatusSignal<Current> statorCurrentAmps;
   private final StatusSignal<Current> supplyCurrentAmps;
   private final StatusSignal<Voltage> motorVoltage;
@@ -50,21 +51,21 @@ public ClimberIO(CANBus canBus) {
     climberMotor = new TalonFX(30, canBus);
     climberMotor.getConfigurator().apply(ClimberIO.getClimberConfiguration());
 
-    angularVelocityRotsPerSec = climberMotor.getVelocity();
+    velocityMetersPerSec = climberMotor.getVelocity();
     supplyCurrentAmps = climberMotor.getSupplyCurrent();
     motorVoltage = climberMotor.getMotorVoltage();
     statorCurrentAmps = climberMotor.getStatorCurrent();
     motorTemp = climberMotor.getDeviceTemp();
-    motorPositionRotations = climberMotor.getPosition();
+    motorPositionMeters = climberMotor.getPosition();
 
     BaseStatusSignal.setUpdateFrequencyForAll(
         50.0,
-        angularVelocityRotsPerSec,
+        velocityMetersPerSec,
         supplyCurrentAmps,
         motorVoltage,
         statorCurrentAmps,
         motorTemp,
-        motorPositionRotations);
+        motorPositionMeters);
     climberMotor.optimizeBusUtilization();
   }
 
@@ -76,7 +77,8 @@ public static TalonFXConfiguration getClimberConfiguration() {
     config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
     // todo: find and make climber gear ratio variable
-    config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO;
+    config.Feedback.SensorToMechanismRatio =
+        ClimberSubsystem.GEAR_RATIO * (Math.PI * ClimberSubsystem.SPOOL_DIAMETER_METERS);
 
     // todo: tune
     config.Slot0.kS = 0.0;
@@ -108,19 +110,23 @@ public void setClimberVelocity(double climberVelocity) {
 
   public void updateInputs(ClimberIOInputs inputs) {
     BaseStatusSignal.refreshAll(
-        motorPositionRotations,
-        angularVelocityRotsPerSec,
+        motorPositionMeters,
+        velocityMetersPerSec,
         statorCurrentAmps,
         supplyCurrentAmps,
         motorVoltage,
         motorTemp);
 
-    inputs.motorPositionRotations =
-        Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble());
+    inputs.motorPositionMeters = motorPositionMeters.getValueAsDouble();
     inputs.motorVoltage = motorVoltage.getValueAsDouble();
     inputs.motorTempC = motorTemp.getValueAsDouble();
     inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble();
     inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble();
-    inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble();
+    inputs.motorVelocityMetersPerSec = velocityMetersPerSec.getValueAsDouble();
+  }
+
+  @AutoLogOutput(key = "Climber/Setpoint Meters")
+  public double getClimberSetpointMeters() {
+    return climberSetpoint;
   }
 }

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

@@ -1,10 +1,13 @@
 package frc.robot.subsystems.climber;
 
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 
 public class ClimberSubsystem extends SubsystemBase {
+  // From CAD
+  public static final double SPOOL_DIAMETER_METERS = Units.inchesToMeters(0.668898);
   // todo: find actual constants
   public static double GEAR_RATIO = (45.0 / 1.0);
   public static double MAX_EXTENSION_METERS = 0.2413;