Climbing and default commands

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

@@ -255,6 +255,15 @@ public static class ShooterConstants {
             MiscConstants.TUNING_MODE);
   }
 
+  public static class ClimberConstants {
+    public static final int CLIMBER_MOTOR_ID = 16;
+    public static final boolean INVERTED = false;
+
+    public static final double SUPPLY_CURRENT_LIMIT = 65;
+    public static final double SUPPLY_CURRENT_THRESHOLD = 40;
+    public static final double SUPPLY_TIME_THRESHOLD = 0.0;
+  }
+
   public static class TeleopConstants {
     private TeleopConstants() {}
 

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

@@ -0,0 +1,73 @@
+package frc.robot.subsystems.climber;
+
+import static frc.robot.Constants.ClimberConstants.*;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.MiscConstants;
+import frc.robot.telemetry.types.DoubleTelemetryEntry;
+import frc.robot.telemetry.types.EventTelemetryEntry;
+import frc.robot.telemetry.wrappers.TelemetryTalonFX;
+import frc.robot.utils.ConfigEquality;
+import frc.robot.utils.ConfigurationUtils;
+import frc.robot.utils.ConfigurationUtils.StringFaultRecorder;
+
+public class ClimberSubsystem extends SubsystemBase {
+  private final TelemetryTalonFX climberMotor =
+      new TelemetryTalonFX(CLIMBER_MOTOR_ID, "/climber/motor", MiscConstants.TUNING_MODE);
+  private final EventTelemetryEntry eventEntry = new EventTelemetryEntry("/climber/events");
+  private final DoubleTelemetryEntry voltageReqEntry =
+      new DoubleTelemetryEntry("/climber/voltageReq", MiscConstants.TUNING_MODE);
+
+  public ClimberSubsystem() {
+    configMotor();
+
+    setDefaultCommand(setVoltageCommand(0.0));
+  }
+
+  // TODO: Make it move with the elevator
+  private void configMotor() {
+    TalonFXConfiguration motorConfig = new TalonFXConfiguration();
+    motorConfig.CurrentLimits.SupplyCurrentLimit = SUPPLY_CURRENT_LIMIT;
+    motorConfig.CurrentLimits.SupplyCurrentThreshold = SUPPLY_CURRENT_THRESHOLD;
+    motorConfig.CurrentLimits.SupplyTimeThreshold = SUPPLY_TIME_THRESHOLD;
+    motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+    motorConfig.MotorOutput.Inverted =
+        INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
+    motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    StringFaultRecorder faultRecorder = new StringFaultRecorder();
+    ConfigurationUtils.applyCheckRecordCTRE(
+        () -> climberMotor.getConfigurator().apply(motorConfig),
+        () -> {
+          TalonFXConfiguration appliedConfig = new TalonFXConfiguration();
+          climberMotor.getConfigurator().refresh(appliedConfig);
+          return ConfigEquality.isTalonConfigurationEqual(motorConfig, appliedConfig);
+        },
+        faultRecorder.run("Motor config"),
+        MiscConstants.CONFIGURATION_ATTEMPTS);
+    ConfigurationUtils.applyCheckRecordCTRE(
+        climberMotor::optimizeBusUtilization,
+        () -> true,
+        faultRecorder.run("Optimize bus utilization"),
+        MiscConstants.CONFIGURATION_ATTEMPTS);
+    ConfigurationUtils.postDeviceConfig(
+        faultRecorder.hasFault(),
+        eventEntry::append,
+        "Climber motor",
+        faultRecorder.getFaultString());
+  }
+
+  public void setVoltage(double voltage) {
+    climberMotor.setControl(new VoltageOut(voltage));
+    voltageReqEntry.append(voltage);
+  }
+
+  public Command setVoltageCommand(double voltage) {
+    return this.run(() -> setVoltage(voltage));
+  }
+}

src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java

@@ -57,6 +57,8 @@ public class ElevatorSubsystem extends SubsystemBase {
 
   public ElevatorSubsystem() {
     configMotors();
+
+    setDefaultCommand(setVoltageCommand(0.0));
   }
 
   private void configMotors() {
@@ -158,6 +160,10 @@ public Command setElevatorPositionCommand(double position) {
             });
   }
 
+  public Command setVoltageCommand(double voltage) {
+    return this.run(() -> setVoltage(voltage));
+  }
+
   public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
     return elevatorVoltageSysId.quasistatic(direction);
   }

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -31,6 +31,7 @@ public class IntakeSubsystem extends SubsystemBase {
 
   public IntakeSubsystem() {
     configMotor();
+    setDefaultCommand(setIntakeVoltageCommand(0.0));
   }
 
   private void configMotor() {

src/main/java/frc/robot/subsystems/slapdown/SlapdownSubsystem.java

@@ -63,6 +63,8 @@ public SlapdownSubsystem() {
     rotationEncoder = rotationMotor.getEncoder();
     rotationFF = ROTATION_FF_GAINS.createFeedforward();
     configMotors();
+
+    setDefaultCommand(stopAllCommand());
   }
 
   private void configMotors() {
@@ -214,6 +216,14 @@ public Command setFeederVoltageCommand(double voltage) {
     return this.run(() -> this.setFeederVoltage(voltage));
   }
 
+  public Command stopAllCommand() {
+    return this.run(
+        () -> {
+          this.setRotationVoltage(0.0);
+          this.setFeederVoltage(0.0);
+        });
+  }
+
   @Override
   public void periodic() {
     rotationMotor.logValues();

src/main/java/frc/robot/subsystems/transport/TransportSubsystem.java

@@ -41,6 +41,7 @@ public void runShooterTransportVoltage(double voltage) {
 
   public TransportSubsystem() {
     configMotor();
+    setDefaultCommand(setVoltageCommand(0.0));
   }
 
   public void configMotor() {
@@ -94,6 +95,10 @@ public Command runTransportOutCommand() {
         () -> this.runShooterTransportVoltage(0));
   }
 
+  public Command setVoltageCommand(double voltage) {
+    return this.run(() -> this.runShooterTransportVoltage(voltage));
+  }
+
   @Override
   public void periodic() {
     transportMotor.logValues();