Climber subsystem implementation with basic PID

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
     id "com.peterabeles.gversion" version "1.10"
     id "org.sonarqube" version "4.4.1.3373"
 }

src/main/java/frc/robot/Constants.java

@@ -99,4 +99,24 @@ private DriveConstants() {
             ModuleConstants.GearRatios.L3
     );
   }
+
+  public class ClimberConstants {
+    private ClimberConstants() {
+      throw new IllegalStateException("Static class should not be constructed");
+    }
+
+    public static final int LEFT_CLIMBER_ID = 18;
+    public static final int RIGHT_CLIMBER_ID = 19;
+
+    public static final double CLIMBER_KP = 0.0;
+    public static final double CLIMBER_KI = 0.0;
+    public static final double CLIMBER_KD = 0.0;
+
+    public static final double CLIMBER_KV = 0.0;
+    public static final double CLIMBER_KS = 0.0;
+    public static final double CLIMBER_KA = 0.0;
+
+    public static final double CLIMBER_GEAR_RATIO = 0.0;
+    public static final double CLIMBER_CONVERSION_FACTOR_INCHES = 0.0;
+  }
 }

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

@@ -24,6 +24,9 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.FeedForwardCharacterization;
+import frc.robot.subsystems.climber.ClimberIO;
+import frc.robot.subsystems.climber.ClimberIOPrototype;
+import frc.robot.subsystems.climber.ClimberSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon1;
@@ -45,6 +48,7 @@ public class RobotContainer {
   // Subsystems
   private final DriveSubsystem m_driveSubsystem;
   private final ShooterSubsystem m_shooter;
+  private final ClimberSubsystem m_climber;
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -64,6 +68,7 @@ public RobotContainer() {
               new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS),
               new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS));
           m_shooter = new ShooterSubsystem(new ShooterIOPrototype());
+          m_climber = new ClimberSubsystem(new ClimberIOPrototype());
         }
     case SIM -> {
       // Sim robot, instantiate physics sim IO implementations
@@ -76,6 +81,7 @@ public RobotContainer() {
             new ModuleIOSim(DriveConstants.BL_MOD_CONSTANTS),
             new ModuleIOSim(DriveConstants.BR_MOD_CONSTANTS));
         m_shooter = new ShooterSubsystem(new ShooterIOPrototype());
+        m_climber = new ClimberSubsystem(new ClimberIOPrototype());
       }
     default -> {
       // Replayed robot, disable IO implementations
@@ -92,6 +98,7 @@ public RobotContainer() {
                         new ModuleIO() {
                         });
         m_shooter = new ShooterSubsystem(new ShooterIO() {});
+        m_climber = new ClimberSubsystem(new ClimberIO() {});
       }
     }
 
@@ -134,8 +141,15 @@ private void configureButtonBindings() {
                             m_driveSubsystem)
                 .ignoringDisable(true));
 
-    controller.a().whileTrue(m_shooter.setShooterPower(0.6, 0.6))
-            .whileFalse(m_shooter.setShooterPower(0.0, 0.0));
+    controller.a().whileTrue(m_shooter.setShooterPowerFactory(0.6, 0.6))
+            .whileFalse(m_shooter.setShooterPowerFactory(0.0, 0.0));
+    controller.b().whileTrue(m_shooter.setIntakePowerFactory(0.75))
+            .whileFalse(m_shooter.setIntakePowerFactory(0.0));
+
+    controller.leftBumper().whileTrue(m_climber.setClimberPowerFactory(-0.25))
+            .whileFalse(m_climber.setClimberPowerFactory(0.0));
+    controller.rightBumper().whileTrue(m_climber.setClimberPowerFactory(0.25))
+            .whileFalse(m_climber.setClimberPowerFactory(0.0));
   }
 
   /**

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

@@ -0,0 +1,105 @@
+package frc.robot.subsystems.climber;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ClimberIO {
+  @AutoLog
+  class ClimberIOInputs {
+    protected double leftClimberPosition = 0.0;
+    protected double rightClimberPosition = 0.0;
+    protected double leftClimberVelocity = 0.0;
+    protected double rightClimberVelocity = 0.0;
+    protected double leftClimberCurrentDraw = 0.0;
+    protected double rightClimberCurrentDraw = 0.0;
+    protected double leftClimberCurrentSetpoint = 0.0;
+    protected double rightClimberCurrentSetpoint = 0.0;
+    protected double leftClimberAppliedOutput = 0.0;
+    protected double rightClimberAppliedOutput = 0.0;
+
+    public double getLeftClimberPosition() {
+      return leftClimberPosition;
+    }
+
+    public void setLeftClimberPosition(double leftClimberPosition) {
+      this.leftClimberPosition = leftClimberPosition;
+    }
+
+    public double getRightClimberPosition() {
+      return rightClimberPosition;
+    }
+
+    public void setRightClimberPosition(double rightClimberPosition) {
+      this.rightClimberPosition = rightClimberPosition;
+    }
+
+    public double getLeftClimberVelocity() {
+      return leftClimberVelocity;
+    }
+
+    public void setLeftClimberVelocity(double leftClimberVelocity) {
+      this.leftClimberVelocity = leftClimberVelocity;
+    }
+
+    public double getRightClimberVelocity() {
+      return rightClimberVelocity;
+    }
+
+    public void setRightClimberVelocity(double rightClimberVelocity) {
+      this.rightClimberVelocity = rightClimberVelocity;
+    }
+
+    public double getLeftClimberCurrentDraw() {
+      return leftClimberCurrentDraw;
+    }
+
+    public void setLeftClimberCurrentDraw(double leftClimberCurrentDraw) {
+      this.leftClimberCurrentDraw = leftClimberCurrentDraw;
+    }
+
+    public double getRightClimberCurrentDraw() {
+      return rightClimberCurrentDraw;
+    }
+
+    public void setRightClimberCurrentDraw(double rightClimberCurrentDraw) {
+      this.rightClimberCurrentDraw = rightClimberCurrentDraw;
+    }
+
+    public double getLeftClimberCurrentSetpoint() {
+      return leftClimberCurrentSetpoint;
+    }
+
+    public void setLeftClimberCurrentSetpoint(double leftClimberCurrentSetpoint) {
+      this.leftClimberCurrentSetpoint = leftClimberCurrentSetpoint;
+    }
+
+    public double getRightClimberCurrentSetpoint() {
+      return rightClimberCurrentSetpoint;
+    }
+
+    public void setRightClimberCurrentSetpoint(double rightClimberCurrentSetpoint) {
+      this.rightClimberCurrentSetpoint = rightClimberCurrentSetpoint;
+    }
+
+    public double getLeftClimberAppliedOutput() {
+      return leftClimberAppliedOutput;
+    }
+
+    public void setLeftClimberAppliedOutput(double leftClimberAppliedOutput) {
+      this.leftClimberAppliedOutput = leftClimberAppliedOutput;
+    }
+
+    public double getRightClimberAppliedOutput() {
+      return rightClimberAppliedOutput;
+    }
+
+    public void setRightClimberAppliedOutput(double rightClimberAppliedOutput) {
+      this.rightClimberAppliedOutput = rightClimberAppliedOutput;
+    }
+  }
+
+  default void setRightVoltage(double volts) {}
+  default void setLeftVoltage(double volts) {}
+  default void setRightPosition(double degrees) {}
+  default void setLeftPosition(double degrees) {}
+  default void updateInputs(ClimberIOInputsAutoLogged inputs) {}
+}

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

@@ -0,0 +1,88 @@
+package frc.robot.subsystems.climber;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import frc.robot.Constants.ClimberConstants;
+import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
+import lib.properties.phoenix6.PidPropertyPublic;
+
+public class ClimberIOPrototype implements ClimberIO {
+  private final TalonFX m_leftTalon;
+  private final TalonFX m_rightTalon;
+  private final PidPropertyPublic m_leftClimberPid;
+  private final PidPropertyPublic m_rightClimberPid;
+
+  public ClimberIOPrototype() {
+    m_leftTalon = new TalonFX(ClimberConstants.LEFT_CLIMBER_ID);
+    m_rightTalon = new TalonFX(ClimberConstants.RIGHT_CLIMBER_ID);
+
+    var rightTalonConfig = new TalonFXConfiguration();
+    rightTalonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    m_rightTalon.getConfigurator().apply(rightTalonConfig);
+
+    var leftTalonConfig = new TalonFXConfiguration();
+    m_leftTalon.getConfigurator().apply(leftTalonConfig);
+
+    m_leftClimberPid = new Phoenix6PidPropertyBuilder(
+        "Climber/Left PID",
+        false, m_leftTalon, 0)
+        .addP(ClimberConstants.CLIMBER_KP)
+        .addI(ClimberConstants.CLIMBER_KI)
+        .addD(ClimberConstants.CLIMBER_KD)
+        .build();
+
+    m_rightClimberPid = new Phoenix6PidPropertyBuilder(
+        "Climber/Right PID",
+        false, m_rightTalon, 0)
+        .addP(ClimberConstants.CLIMBER_KP)
+        .addI(ClimberConstants.CLIMBER_KI)
+        .addD(ClimberConstants.CLIMBER_KD)
+        .build();
+
+  }
+
+  @Override
+  public void setLeftVoltage(double volts) {
+    m_leftTalon.setVoltage(volts);
+  }
+
+  @Override
+  public void setRightVoltage(double volts) {
+    m_rightTalon.setVoltage(volts);
+  }
+
+  @Override
+  public void setLeftPosition(double degrees) {
+    final PositionVoltage request = new PositionVoltage(0).withSlot(0);
+    m_leftTalon.setControl(request.withPosition(degrees / 360.0));
+  }
+
+  @Override
+  public void setRightPosition(double degrees) {
+    final PositionVoltage request = new PositionVoltage(0).withSlot(0);
+    m_rightTalon.setControl(request.withPosition(degrees / 360.0));
+  }
+
+  @Override
+  public void updateInputs(ClimberIOInputsAutoLogged inputs) {
+    m_leftClimberPid.updateIfChanged();
+    m_rightClimberPid.updateIfChanged();
+
+    inputs.setLeftClimberPosition(m_leftTalon.getPosition().getValueAsDouble());
+    inputs.setRightClimberPosition(m_rightTalon.getPosition().getValueAsDouble());
+
+    inputs.setLeftClimberVelocity(m_leftTalon.getVelocity().getValueAsDouble());
+    inputs.setRightClimberVelocity(m_rightTalon.getVelocity().getValueAsDouble());
+
+    inputs.setLeftClimberCurrentDraw(m_leftTalon.getSupplyCurrent().getValueAsDouble());
+    inputs.setRightClimberCurrentDraw(m_rightTalon.getSupplyCurrent().getValueAsDouble());
+
+    inputs.setLeftClimberCurrentSetpoint(m_leftTalon.getClosedLoopReference().getValueAsDouble());
+    inputs.setRightClimberCurrentSetpoint(m_rightTalon.getClosedLoopReference().getValueAsDouble());
+
+    inputs.setLeftClimberAppliedOutput(m_leftTalon.getBridgeOutput().getValueAsDouble());
+    inputs.setRightClimberAppliedOutput(m_rightTalon.getBridgeOutput().getValueAsDouble());
+  }
+}

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

@@ -0,0 +1,31 @@
+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 {
+  private final ClimberIO m_io;
+  private final ClimberIOInputsAutoLogged m_inputs;
+  public ClimberSubsystem(ClimberIO io) {
+    m_io = io;
+    m_inputs = new ClimberIOInputsAutoLogged();
+  }
+
+  @Override
+  public void periodic() {
+    m_io.updateInputs(m_inputs);
+    Logger.processInputs("Climber", m_inputs);
+  }
+
+  public void setClimberPower(double power) {
+    m_io.setRightVoltage(power * 12.0);
+    m_io.setLeftVoltage(power * 12.0);
+  }
+
+  public Command setClimberPowerFactory(double power) {
+    return run(() -> setClimberPower(power));
+  }
+}
+

src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java

@@ -18,6 +18,16 @@ public record ModuleConstants(
     // Turning loop gains
     double TURN_KP, double TURN_KI, double TURN_KD) {
   public static final double DEFAULT_WHEEL_RADIUS_METERS = Units.inchesToMeters(2.0);
+  public static final ModuleConstants BLANK_CONSTANTS = new ModuleConstants(
+          0,
+          new int[] {0, 0, 0},
+          new double[] {0.0, 0.0, 0.0},
+          new double[] {0.0, 0.0, 0.0},
+          new double[] {0.0, 0.0, 0.0},
+          0.0,
+          false,
+          GearRatios.L3
+  );
 
   public enum GearRatios {
     TURN(150.0 / 7.0),

src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java

@@ -139,6 +139,6 @@ default void setTurnBrakeMode(boolean enable) {}
 
   // Used to pass moduleConstants
   default ModuleConstants getModuleConstants() {
-    throw new UnsupportedOperationException("getModuleConstants() not implemented");
+    return ModuleConstants.BLANK_CONSTANTS;
   }
 }