Add Climber stuff

src/main/java/frc4388/robot/RobotContainer.java

@@ -179,43 +179,13 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
 
-            
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-        //     .onTrue(new RotTo45(m_robotSwerveDrive));
-
-        // new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
-        //     .onTrue(RobotShoot)
-        //     .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter));
-
-        // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
-        //     .onTrue(RobotIntake)
-        //     .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter));
-            
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-        //     .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); 
-
         new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
 
-       
+    
-        
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-        //     .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); 
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-        //     .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); 
-            
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-        //     .onTrue(new InstantCommand(() -> {;}, m_robotIntake)); 
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
-        //     .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); 
-
-
-
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
-        //     .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
             .onTrue(new InstantCommand(()  -> {
@@ -237,9 +207,6 @@ public class RobotContainer {
             .onFalse(new InstantCommand(() -> {
                 m_robotSwerveDrive.softStop();
             }));
-            // .onTrue(new InstantCommand(() -> {
-            //     m_robotIntake.setMode(IntakeMode.Extended);
-            // }))
 
         // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
         new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
@@ -252,15 +219,6 @@ public class RobotContainer {
                     );
                 }, m_robotSwerveDrive)
             );
-            // .onTrue(new InstantCommand(() -> {
-            //     // When Right trigger is pressed, 
-            //     m_robotShooter.setShooterReady();
-            // }))
-            // .onFalse(new InstantCommand(() -> {
-            //     m_robotIntake.setMode(IntakeMode.Retracted);
-            //     m_robotShooter.setShooterNotReady();
-            // }));
-
 
         // D-PAD fine alignment
         new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
@@ -302,7 +260,14 @@ public class RobotContainer {
             })).onFalse(new InstantCommand(() -> {
                 m_robotShooter.setShooterNOTShoot();
             }));
-    
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Extending);
+            }))
+            .onFalse(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Idle);
+            }));
 
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
             .onTrue(new InstantCommand(() -> {
@@ -312,13 +277,10 @@ public class RobotContainer {
                 m_robotIntake.setMode(IntakeMode.Idle);
             }));
 
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> {
+        //     .onTrue(new InstantCommand(() -> {
-                m_robotIntake.setMode(IntakeMode.Extending);
+        //         m_robotClimber.toggleDeployed();
-            }))
+        //     }));
-            .onFalse(new InstantCommand(() -> {
-                m_robotIntake.setMode(IntakeMode.Idle);
-            }));
 
 
    }

src/main/java/frc4388/robot/subsystems/climber/Climber.java

@@ -1,50 +1,58 @@
 package frc4388.robot.subsystems.climber;
 
+import static edu.wpi.first.units.Units.Inches;
+
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Climber extends SubsystemBase {
-    ClimberIO io;
+    public ClimberIO io;
     ClimberStateAutoLogged state = new ClimberStateAutoLogged();
 
-    // Supplier<Pose2d> m_swervePoseSupplier;
-
     public Climber(
         ClimberIO io
-        // Supplier<Pose2d> swervePoseSupplier
     ) {
         this.io = io;
-        // this.m_swervePoseSupplier = swervePoseSupplier;
     }
 
-    // public enum ClimberMode {
+    ClimberMode mode = ClimberMode.Retracting;
-    //     Up,
-    //     Down,
-    // }
 
-    // public void setMode(IntakeMode mode) {
+    public enum ClimberMode {
-    //     switch (mode) {
+        Extended,
-    //         case Up:
+        Retracting,
-    //             io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
+    }
-    //             io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP);
-    //             break;
-    //         case Down:
-    //             io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
-    //             io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
-    //             break;
-    //     }
-    // }
 
+    public void deploy() {
+        mode = ClimberMode.Extended;
+    }
 
+    public void retract() {
+        mode = ClimberMode.Retracting;
+    }
+
+    public void toggleDeployed() {
+        switch (mode) {
+            case Extended:
+                mode = ClimberMode.Retracting;
+                break;
+            case Retracting:
+                mode = ClimberMode.Extended;
+                break;
+        }
+    }    
 
     @Override
     public void periodic() {
         
-        
+        switch (mode) {
-
+            case Extended:
-        // FaultReporter.register(this); // TODO Implement fault reporter
+                io.setClimberDistance(state, Inches.of(ClimberConstants.CLIMBER_RETRACT_SPEED.get()));
-
+                break;
+            case Retracting:
+                io.setPercentOutput(state, ClimberConstants.CLIMBER_RETRACT_SPEED.get());
+                break;
+        }
 
         Logger.processInputs("Climber", state);
 

src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java

@@ -8,36 +8,32 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.units.measure.Distance;
+import frc4388.utility.configurable.ConfigurableDouble;
+import frc4388.utility.status.CanDevice;
 
 public class ClimberConstants {
     // Motor conversions
-
+    public static final double CLIMBER_MOTOR_GEAR_RATIO = 1.; // dimentionless
-    public static final double CLIMBER_MOTOR_GEAR_RATIO = 1.;
+    public static final double CLIMBER_SPOOL_DIAMETER = 1.; // in
+    public static final double CLIMBER_REVS_PER_INCH = CLIMBER_MOTOR_GEAR_RATIO / (Math.PI * CLIMBER_SPOOL_DIAMETER); // in
 
     //IDs
+    public static final CanDevice CLIMBER_MOTOR = new CanDevice("Climber Motor", 30);
+    public static final int CLIMBER_LIMIT_SWITCH_CHANNEL = 8;
 
-
+    // Constants
-    // public static final CanDevice CLIMBER_ID   = new CanDevice("SHOOTER 1", 20);
-        
-    // Limits
-
-    // 0 is the forward angle on the robot.
-    // negative is left, positive is right
-    // public static final Angle L1 = Degrees.of(-180);
-    // public static final Angle ARM_LIMIT_UPPER = Degrees.of(180);
     public static final Distance CLIMBER_LIMIT_LOWER = Inches.of(0);
-
+    public static final ConfigurableDouble CLIMBER_LIMIT_UPPER = new ConfigurableDouble("Climber Height in", 6.);
-    public static final Distance CLIMBER_LIMIT_UPPER = Inches.of(0);
+    public static final ConfigurableDouble CLIMBER_RETRACT_SPEED = new ConfigurableDouble("Climber retract %", 0.3);
-
 
     public static final Slot0Configs CLIMBER_PID = new Slot0Configs()
-        .withKP(2.0)
+        .withKP(0.3)
         .withKI(0.0)
-        .withKD(10.0);
+        .withKD(0.0);
 
-    // 0 is paralell to the ground, 90 is directly up
+    public static final ConfigurableDouble CLIMBER_kP = new ConfigurableDouble("Climber kP", 0.3);
-    // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
+    public static final ConfigurableDouble CLIMBER_kI = new ConfigurableDouble("Climber kI", 0.);
-    // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
+    public static final ConfigurableDouble CLIMBER_kD = new ConfigurableDouble("Climber kD", 0.);
     
     // Motor configs
     public static final TalonFXConfiguration CLIMBER_MOTOR_CONFIG = new TalonFXConfiguration()
@@ -50,18 +46,4 @@ public class ClimberConstants {
                     .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
                     .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
     );
-}
+}
-
-    // public static final class IDs {
-    //     public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22);
-    // }
-    // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
-    //     .withCurrentLimits(
-    //         new CurrentLimitsConfigs()
-    //             .withStatorCurrentLimit(40) // TODO: tune???
-    //             .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
-    //         ).withMotorOutput(
-    //             new MotorOutputConfigs()
-    //                 .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop
-    //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
-    // );

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

@@ -14,23 +14,13 @@ public interface ClimberIO {
         Distance climberDistance = Inches.of(0);
         Distance climberTargetDistance = Inches.of(0);
         Current climberMotorCurrent = Amps.of(0);
-
+        boolean climberLimit = false;
-        // Distance shooterPitch = Rotations.of(0);
-        // Angle shooterTargetPitch = Rotations.of(0);
-        // Current pitchMotorCurrent = Amps.of(0);
-
-        // AngularVelocity rollerVelocity = RotationsPerSecond.of(0);
-        // AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0);
-        // Current rollerMotorCurrent = Amps.of(0);
-
-        // LinearVelocity feederVelocity = InchesPerSecond.of(0);
-        // LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
-        // Current feederMotorCurrent = Amps.of(0);
     }
 
-    // public default void setShooterAngle(ShooterState state, Angle angle) {}
-    // public default void setShooterPitch(ShooterState state, Angle angle) {}
     public default void setClimberDistance(ClimberState state, Distance distance) {}
+    public default void setPercentOutput(ClimberState state, double percent) {}
 
     public default void updateInputs(ClimberState state) {}
+
+    public default void updateGains() {}
 }

src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java

@@ -2,68 +2,77 @@ package frc4388.robot.subsystems.climber;
 
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Rotations;
+
+import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
 
 import edu.wpi.first.units.measure.*;
+import edu.wpi.first.wpilibj.DigitalInput;
 
 public class ClimberReal implements ClimberIO {
-
-
     TalonFX m_climberMotor;
+    DigitalInput m_lowerLimit;
+
+    PositionDutyCycle climberPosition = new PositionDutyCycle(0);
+    DutyCycleOut climberPercentOutout = new DutyCycleOut(0);
 
     public ClimberReal(
-
+        TalonFX climberMotor,
-        TalonFX climberMotor
+        DigitalInput lowerLimit
     ) {
-        // m_angleMotor = angleMotor;
-        // m_pitchMotor = pitchMotor;
         m_climberMotor = climberMotor;
+        m_lowerLimit = lowerLimit;
+
         // Apply the configs
         m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_PID);
         m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_MOTOR_CONFIG);
 
+        climberPosition.Slot = 0;
     }
 
-    private Distance clampDistance(Distance distance, Distance climberLimitLower, Distance climberLimitUpper){
-            if(distance.gt(climberLimitUpper)) {
-                return climberLimitUpper;
-            }else if(distance.lt(climberLimitLower)) {
-                return climberLimitLower;
-            }else{
-                return distance;
-        }
-    }
-
-
     
 
     @Override
     public void setClimberDistance(ClimberState state, Distance distance) {
         state.climberTargetDistance = distance;
-        // Assume that the angle is always accurate, because I think we will use a shaft encoder
+
-        // Assume that 0 degrees = forwards. Might need an offset here
+        // length * (motor rot / length) = motor rot
-        
+        double angle = distance.in(Inches) * ClimberConstants.CLIMBER_REVS_PER_INCH;
-        Distance boundedDistance = clampDistance(distance, ClimberConstants.CLIMBER_LIMIT_LOWER, ClimberConstants.CLIMBER_LIMIT_UPPER);
+
-        // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
+        m_climberMotor.setControl(
-        double motorTargetDistance = boundedDistance.in(Inches) / ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO;
+            climberPosition.withPosition(Rotations.of(angle))
-        PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetDistance);
+            .withLimitReverseMotion(m_lowerLimit.get())
-        m_climberMotor.setControl(posRequest);
+        );
+    }
+
+    @Override
+    public void setPercentOutput(ClimberState state, double percent) {
+        // var d = new DutyCycleOut(0);
+        m_climberMotor.setControl(
+            climberPercentOutout.withOutput(percent)
+                .withLimitReverseMotion(m_lowerLimit.get())
+            );
     }
 
     @Override
     public void updateInputs(ClimberState state) {
+        // motor rot / (motor rot / length) = length
+
         double motorRotations = m_climberMotor.getPosition().getValue().in(Rotations);
-        double linearInches = motorRotations * ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO;
+        double linearInches = motorRotations / ClimberConstants.CLIMBER_REVS_PER_INCH;
         state.climberDistance = Inches.of(linearInches);
+
         state.climberMotorCurrent = m_climberMotor.getStatorCurrent(false).getValue();
-
+        state.climberLimit = m_lowerLimit.get();
-        // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
-        // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
-
-        // state.armAngle = m_armMotor.getPosition().getValue();
-        // state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue();
-
 
     }
+
+        @Override 
+    public void updateGains() {
+        ClimberConstants.CLIMBER_PID.kP = ClimberConstants.CLIMBER_kP.get();
+        ClimberConstants.CLIMBER_PID.kI = ClimberConstants.CLIMBER_kI.get();
+        ClimberConstants.CLIMBER_PID.kD = ClimberConstants.CLIMBER_kD.get();
+        m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_PID);
+    }
 }

src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java

@@ -19,7 +19,7 @@ public class IntakeReal implements IntakeIO {
     DigitalInput m_armLimitSwitch;
 
     PositionDutyCycle armPosition = new PositionDutyCycle(0);
-    // VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0);
+    DutyCycleOut armPercentOutput = new DutyCycleOut(0);
 
     public IntakeReal(
         DigitalInput armLimitSwitch,
@@ -94,9 +94,8 @@ public class IntakeReal implements IntakeIO {
 
     @Override
     public void armOutput(double percentOutput){
-        var d = new DutyCycleOut(0);
         m_armMotor.setControl(
-            d.withOutput(percentOutput)
+            armPercentOutput.withOutput(percentOutput)
                 .withLimitReverseMotion(!m_armLimitSwitch.get())
             );
     }