Reworked deep climb

src/main/java/frc/robot/RobotContainer.java

@@ -286,8 +286,8 @@ private void configureBindings() {
         // CORAL.and(leftDriveController.getTrigger()).whileTrue(alignToReef(9, 0));
 
         // Climb Bindings
-        leftDriveController.getLeftThumb().whileTrue(climberSubsystem.moveClimberDown());
-        leftDriveController.getRightThumb().whileTrue(climberSubsystem.moveClimberUp());
+        leftDriveController.getLeftThumb().whileTrue(climberSubsystem.downPosition());
+        leftDriveController.getRightThumb().whileTrue(climberSubsystem.upPosition());
 
         // Intake Bindings
         rightDriveController.getLeftThumb().whileTrue(intakeSubsystem.intake());
@@ -302,6 +302,10 @@ private void configureBindings() {
         ALGAE.and(rightDriveController.getTrigger()).whileTrue(gripperSubsystem.ejectSpinAlgae());
 
         // Technical Bindings
+
+        leftDriveController.getLeftBottomMiddle().onTrue(climberSubsystem.zeroClimberCommand());
+        leftDriveController.getLeftTopMiddle().whileTrue(climberSubsystem.climberTuneable());
+
         rightDriveController
                 .getLeftTopLeft()
                 .onTrue(Commands.runOnce(() -> drivetrain.seedFieldCentric()));

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

@@ -1,5 +1,6 @@
 package frc.robot.constants;
 
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 
@@ -19,4 +20,14 @@ public class ClimberConstants {
                     .withMotionMagicAcceleration(4) // these are guesses, come back here
                     .withMotionMagicCruiseVelocity(4) // also guess
                     .withMotionMagicJerk(4);
+
+    public static final CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs();
+
+    public static final double upperLimit = 100;
+
+    public static final double lowerLimit = 0;
+
+    public static final int id = 83;
+
+    public static final String CANbus = "CANivore";
 }

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

@@ -8,8 +8,6 @@ public interface ClimberIO {
 
     @AutoLog
     public class ClimberIOInputs {
-
-        // public double voltage = 0;
         public double position = 0;
         public double speed = 0;
         public double voltage;
@@ -21,5 +19,5 @@ public class ClimberIOInputs {
 
     public void setVoltage(double voltage);
 
-    public double getPosition();
+    public void resetPosition(double position);
 }

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

@@ -22,11 +22,10 @@ public void setVoltage(double voltage) {
     }
 
     public void setPosition(double position) {
-
         this.positionSetpoint = position;
     }
 
-    public double getPosition() {
-        return position;
+    public void resetPosition(double position) {
+        this.position = position;
     }
 }

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

@@ -3,14 +3,14 @@
 import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfigurator;
-import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import frc.robot.constants.ClimberConstants;
 
 public class ClimberIOTalonFX implements ClimberIO {
     // TBD: Hardcode IDs or add support to make changeable in method
-    private final TalonFX climbermotor = new TalonFX(83, "CANivore");
+    private final TalonFX climbermotor = new TalonFX(ClimberConstants.id, ClimberConstants.CANbus);
     private final MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0);
 
     public ClimberIOTalonFX() {
@@ -20,33 +20,47 @@ public ClimberIOTalonFX() {
 
         TalonFXConfigurator talonConfig = climbermotor.getConfigurator();
 
-        SoftwareLimitSwitchConfigs softwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs();
+        SoftwareLimitSwitchConfigs softwareLimitSwitchConfigs =
+                new SoftwareLimitSwitchConfigs()
+                        .withForwardSoftLimitEnable(true)
+                        .withForwardSoftLimitThreshold(ClimberConstants.upperLimit)
+                        .withReverseSoftLimitThreshold(ClimberConstants.lowerLimit)
+                        .withReverseSoftLimitEnable(true);
 
         talonConfig.apply(
                 new TalonFXConfiguration()
                         .withSoftwareLimitSwitch(softwareLimitSwitchConfigs)
                         .withSlot0(ClimberConstants.slot0Configs)
-                        .withMotionMagic(ClimberConstants.motionMagicConfigs));
+                        .withMotionMagic(ClimberConstants.motionMagicConfigs)
+                        .withCurrentLimits(ClimberConstants.currentLimitsConfigs));
 
-        climbermotor.setControl(new Follower(climbermotor.getDeviceID(), false));
+        climbermotor.setNeutralMode(NeutralModeValue.Brake);
     }
 
     public void updateInputs(ClimberIOInputs inputs) {
-
-        inputs.position = climbermotor.getPosition().refresh().getValueAsDouble();
-        inputs.voltage = climbermotor.getMotorVoltage().refresh().getValueAsDouble();
-        inputs.speed = climbermotor.getVelocity().refresh().getValueAsDouble();
+        inputs.position = climbermotor.getPosition().getValueAsDouble();
+        inputs.voltage = climbermotor.getMotorVoltage().getValueAsDouble();
+        inputs.speed = climbermotor.getVelocity().getValueAsDouble();
+        inputs.temperature = climbermotor.getDeviceTemp().getValueAsDouble();
+        inputs.current = climbermotor.getStatorCurrent().getValueAsDouble();
     }
 
     public void setVoltage(double voltage) {
         climbermotor.setVoltage(voltage);
     }
 
     public void setPosition(double position) {
+        if (position > ClimberConstants.upperLimit) {
+            position = ClimberConstants.upperLimit;
+        }
+        if (position < ClimberConstants.lowerLimit) {
+            position = ClimberConstants.lowerLimit;
+        }
+
         climbermotor.setControl(motionMagicVoltage.withPosition(position));
     }
 
-    public double getPosition() {
-        return climbermotor.getPosition().refresh().getValueAsDouble();
+    public void resetPosition(double position) {
+        climbermotor.setPosition(position);
     }
 }