18 basic climb

src/main/java/org/team5924/frc2026/Constants.java

@@ -191,4 +191,43 @@ public final class ShooterRoller {
             .withInverted(InvertedValue.CounterClockwise_Positive)
             .withNeutralMode(NeutralModeValue.Brake));
   }
+  public final class Climb {
+    public static final int CAN_ID = 0;
+    public static final String BUS = "rio";
+    public static final double REDUCTION = 1.0;
+    public static final double SIM_MOI = 0.001;
+
+    public static final TalonFXConfiguration CONFIG =
+      new TalonFXConfiguration()
+        .withCurrentLimits(
+          new CurrentLimitsConfigs()
+            .withSupplyCurrentLimit(35)
+            .withStatorCurrentLimit(35))
+        .withMotorOutput(
+          new MotorOutputConfigs()
+            .withInverted(InvertedValue.CounterClockwise_Positive)
+            .withNeutralMode(NeutralModeValue.Brake));
+
+    public static final CANdiConfiguration CANDI_CONFIG =
+      new CANdiConfiguration()
+        .withDigitalInputs(
+          new DigitalInputsConfigs()
+            .withS1CloseState(S1CloseStateValue.CloseWhenLow)
+            .withS2CloseState(S2CloseStateValue.CloseWhenLow));
+
+    public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS =
+      new OpenLoopRampsConfigs()
+        .withDutyCycleOpenLoopRampPeriod(0.02)
+        .withTorqueOpenLoopRampPeriod(0.02)
+        .withVoltageOpenLoopRampPeriod(0.02);
+
+    public static final ClosedLoopRampsConfigs CLOSED_LOOP_RAMPS_CONFIGS =
+      new ClosedLoopRampsConfigs()
+        .withDutyCycleClosedLoopRampPeriod(0.02)
+        .withTorqueClosedLoopRampPeriod(0.02)
+        .withVoltageClosedLoopRampPeriod(0.02);
+  }
+
 }
+
+

src/main/java/org/team5924/frc2026/RobotState.java

@@ -21,6 +21,7 @@
 import lombok.Getter;
 import lombok.Setter;
 import org.littletonrobotics.junction.AutoLogOutput;
+import org.team5924.frc2026.subsystems.climb.Climb.ClimbState;
 import org.team5924.frc2026.subsystems.exampleSystem.ExampleSystem.ExampleSystemState;
 import org.team5924.frc2026.subsystems.rollers.exampleRoller.ExampleRoller.ExampleRollerState;
 import org.team5924.frc2026.subsystems.rollers.hopperAgitator.Hopper.HopperState;
@@ -61,6 +62,8 @@ public static RobotState getInstance() {
   /* ### Example Roller ### */
   @Getter @Setter private ExampleRollerState exampleRollerState = ExampleRollerState.IDLE;
 
+  /* ### Climb ### */
+  @Getter @Setter private ClimbState climbState = ClimbState.STOW;
   /*### Shooter Roller ### */
   @Getter @Setter private ShooterRollerState shooterRollerState = ShooterRollerState.OFF;
 }

src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java

@@ -0,0 +1,111 @@
+/*
+ * Climb.java
+ */
+
+/* 
+ * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates.
+ *
+ * This file, and the associated project, are offered under the GNU General
+ * Public License v3.0. A copy of this license can be found in LICENSE.md
+ * at the root of this project.
+ *
+ * If this file has been separated from the original project, you should have
+ * received a copy of the GNU General Public License along with it.
+ * If you did not, see <https://www.gnu.org/licenses>.
+ */
+
+package org.team5924.frc2026.subsystems.climb;
+
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import lombok.Getter;
+
+import java.util.function.DoubleSupplier;
+
+import org.littletonrobotics.junction.Logger;
+import org.team5924.frc2026.RobotState;
+import org.team5924.frc2026.util.Elastic;
+import org.team5924.frc2026.util.Elastic.Notification;
+import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel;
+import org.team5924.frc2026.util.LoggedTunableNumber;
+
+public class Climb extends SubsystemBase {
+
+  private final ClimbIO io;
+  private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged();
+
+  public enum ClimbState {
+    STOW(new LoggedTunableNumber("Climb/Stow", 0)),
+    LEVEL_ONE(new LoggedTunableNumber("Climb/LevelOne", 0)),
+    LEVEL_TWO(new LoggedTunableNumber("Climb/LevelTwo", 0)),
+    LEVEL_THREE(new LoggedTunableNumber("Climb/LevelThree", 0)),
+    CLIMB_DOWN(new LoggedTunableNumber("Climb/ClimbDown", 0)),
+    DEPLOY(new LoggedTunableNumber("Climb/Deploy", 0)),
+    DROP(new LoggedTunableNumber("Climb/Drop", 0)),
+    MOVING(() -> 0.0),
+    // voltage at which the climb subsystem motor moves when controlled by the operator
+    OPERATOR_CONTROL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5));
+
+    private final DoubleSupplier rads;
+
+    ClimbState(DoubleSupplier rads) {
+      this.rads = rads;
+    }
+  }
+
+  @Getter private ClimbState goalState;
+
+  private final Alert climbMotorDisconnected;
+  private final Notification climbMotorDisconnectedNotification;
+  private boolean wasClimbMotorConnected = true;
+
+  public Climb(ClimbIO io) {
+    this.io = io;
+    this.goalState = ClimbState.STOW;
+    this.climbMotorDisconnected =
+        new Alert("Climb System Motor Disconnected!", Alert.AlertType.kWarning);
+    this.climbMotorDisconnectedNotification =
+        new Notification(NotificationLevel.WARNING, "Climb System Motor Disconnected", "");
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("Climb", inputs);
+
+    Logger.recordOutput("Climb/GoalState", goalState.toString());
+    Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState());
+    Logger.recordOutput("Climb/TargetRads", goalState.rads.getAsDouble());
+
+    climbMotorDisconnected.set(!inputs.climbMotorConnected);
+
+    // prevents error spam
+    if (!inputs.climbMotorConnected && wasClimbMotorConnected) {
+      Elastic.sendNotification(climbMotorDisconnectedNotification);
+    }
+
+    wasClimbMotorConnected = inputs.climbMotorConnected;
+  }
+
+  public void runVolts(double volts) {
+    io.runVolts(volts);
+  }
+
+  public void setGoalState(ClimbState goalState) {
+    this.goalState = goalState;
+    switch (goalState) {
+      case OPERATOR_CONTROL:
+        RobotState.getInstance().setClimbState(ClimbState.OPERATOR_CONTROL);
+        break;
+      case MOVING:
+        DriverStation.reportError(
+            "Climb: MOVING is an invalid goal state; it is a transition state!!", null);
+        break;
+      default:
+        RobotState.getInstance().setClimbState(goalState);
+        io.setPosition(goalState.rads.getAsDouble());
+        break;
+    }
+  }
+}

src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java

@@ -0,0 +1,51 @@
+/*
+ * ClimbIO.java
+ */
+
+/* 
+ * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates.
+ *
+ * This file, and the associated project, are offered under the GNU General
+ * Public License v3.0. A copy of this license can be found in LICENSE.md
+ * at the root of this project.
+ *
+ * If this file has been separated from the original project, you should have
+ * received a copy of the GNU General Public License along with it.
+ * If you did not, see <https://www.gnu.org/licenses>.
+ */
+
+package org.team5924.frc2026.subsystems.climb;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ClimbIO {
+  @AutoLog
+  public static class ClimbIOInputs {
+    public boolean climbMotorConnected = true;
+    public double climbPositionRads = 0.0;
+    public double climbVelocityRadsPerSec = 0.0;
+    public double climbAppliedVoltage = 0.0;
+    public double climbSupplyCurrentAmps = 0.0;
+    public double climbTorqueCurrentAmps = 0.0;
+    public double climbTempCelsius = 0.0;
+  }
+
+  /**
+   * Updates the inputs object with the latest data from hardware
+   *
+   * @param inputs Inputs to update
+   */
+  public default void updateInputs(ClimbIOInputs inputs) {}
+
+  /**
+   * Sets the subsystem motor to the specified voltage
+   *
+   * @param volts number of volts
+   */
+  public default void runVolts(double volts) {}
+
+  public default void setPosition(double rads) {}
+
+  /** stops the motor */
+  default void stop() {}
+}

src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOInputs.java

@@ -0,0 +1,28 @@
+/*
+ * ClimbIOInputs.java
+ */
+
+/* 
+ * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates.
+ *
+ * This file, and the associated project, are offered under the GNU General
+ * Public License v3.0. A copy of this license can be found in LICENSE.md
+ * at the root of this project.
+ *
+ * If this file has been separated from the original project, you should have
+ * received a copy of the GNU General Public License along with it.
+ * If you did not, see <https://www.gnu.org/licenses>.
+ */
+
+package org.team5924.frc2026.subsystems.climb;
+
+public class ClimbIOInputs {
+
+  public boolean climbMotorConnected;
+  public Object climbVelocityRadsPerSec;
+  public double climbAppliedVoltage;
+  public double climbSupplyCurrentAmps;
+  public double climbTorqueCurrentAmps;
+  public double climbTempCelsius;
+  public Object climbPositionRads;
+}

src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java

@@ -0,0 +1,107 @@
+/*
+ * ClimbIOTalonFX.java
+ */
+
+/* 
+ * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates.
+ *
+ * This file, and the associated project, are offered under the GNU General
+ * Public License v3.0. A copy of this license can be found in LICENSE.md
+ * at the root of this project.
+ *
+ * If this file has been separated from the original project, you should have
+ * received a copy of the GNU General Public License along with it.
+ * If you did not, see <https://www.gnu.org/licenses>.
+ */
+
+package org.team5924.frc2026.subsystems.climb;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.math.util.Units;
+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.team5924.frc2026.Constants;
+
+public class ClimbIOTalonFX implements ClimbIO {
+
+  private final TalonFX climbTalon;
+  private final StatusSignal<Angle> climbPosition;
+  private final StatusSignal<AngularVelocity> climbVelocity;
+  private final StatusSignal<Voltage> climbAppliedVoltage;
+  private final StatusSignal<Current> climbSupplyCurrent;
+  private final StatusSignal<Current> climbTorqueCurrent;
+  private final StatusSignal<Temperature> climbTempCelsius;
+
+  // Single shot for voltage mode, robot loop will call continuously
+  private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0);
+  private final PositionVoltage positionOut =
+      new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true);
+
+  public ClimbIOTalonFX() {
+    climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS));
+    climbTalon.getConfigurator().apply(Constants.Climb.CONFIG);
+
+    // Get select status signals and set update frequency
+    climbPosition = climbTalon.getPosition();
+    climbVelocity = climbTalon.getVelocity();
+    climbAppliedVoltage = climbTalon.getMotorVoltage();
+    climbSupplyCurrent = climbTalon.getSupplyCurrent();
+    climbTorqueCurrent = climbTalon.getTorqueCurrent();
+    climbTempCelsius = climbTalon.getDeviceTemp();
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        50.0,
+        climbPosition,
+        climbVelocity,
+        climbAppliedVoltage,
+        climbSupplyCurrent,
+        climbTorqueCurrent,
+        climbTempCelsius);
+
+    climbTalon.setPosition(0);
+  }
+
+  @Override
+  public void updateInputs(ClimbIOInputs inputs) {
+    inputs.climbMotorConnected =
+        BaseStatusSignal.refreshAll(
+                climbPosition,
+                climbVelocity,
+                climbAppliedVoltage,
+                climbSupplyCurrent,
+                climbTorqueCurrent,
+                climbTempCelsius)
+            .isOK();
+    inputs.climbPositionRads =
+        Units.rotationsToRadians(climbPosition.getValueAsDouble()) / Constants.Climb.REDUCTION;
+    inputs.climbVelocityRadsPerSec =
+        Units.rotationsToRadians(climbVelocity.getValueAsDouble()) / Constants.Climb.REDUCTION;
+    inputs.climbAppliedVoltage = climbAppliedVoltage.getValueAsDouble();
+    inputs.climbSupplyCurrentAmps = climbSupplyCurrent.getValueAsDouble();
+    inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble();
+    inputs.climbTempCelsius = climbTempCelsius.getValueAsDouble();
+  }
+
+  @Override
+  public void runVolts(double volts) {
+    climbTalon.setControl(voltageOut.withOutput(volts));
+  }
+
+  @Override
+  public void stop() {
+    climbTalon.stopMotor();
+  }
+
+  @Override
+  public void setPosition(double rads) {
+    climbTalon.setControl(positionOut.withPosition(rads * Constants.Climb.REDUCTION * Units.radiansToRotations(1.0)));
+  }
+}