18 basic climb

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

@@ -1,6 +1,7 @@
-/*
+
+/* 
  * Constants.java
- */
+*/
 
 /* 
  * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates.
@@ -529,8 +530,7 @@ public final class TurretRight {
           new MotorOutputConfigs()
             .withInverted(InvertedValue.Clockwise_Positive)
             .withNeutralMode(NeutralModeValue.Brake));
-
-    public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS =
+       public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS =
       new OpenLoopRampsConfigs()
         .withDutyCycleOpenLoopRampPeriod(0.02)
         .withTorqueOpenLoopRampPeriod(0.02)
@@ -566,4 +566,64 @@ public final class TurretRight {
         .withSensorDirection(SensorDirectionValue.Clockwise_Positive);
   }
 
+  public final class Climb { // TODO: update these values
+    public static final int CAN_ID = 60;
+    public static final String BUS = "rio";
+    public static final double MOTOR_TO_CANCODER = (7.0 / 1.0); // TODO: Update Reductions and add hook to reduction
+    public static final double CANCODER_TO_MECHANISM = (7.0 / 1.0);
+    public static final double MOTOR_TO_MECHANISM = MOTOR_TO_CANCODER * CANCODER_TO_MECHANISM;
+    public static final double SIM_MOI = 0.001;
+
+    public static final double MIN_POSITION_MULTI = 0.8; // rotations
+    public static final double MAX_POSITION_MULTI = 0.8; // rotations
+
+    public static final double MIN_POSITION_RADS = -Math.PI * MIN_POSITION_MULTI;
+    public static final double MAX_POSITION_RADS = Math.PI * MAX_POSITION_MULTI;
+
+
+    public static final double EPSILON_RADS = Units.degreesToRadians(2.0); 
+    public static final double JOYSTICK_DEADZONE = 0.05;
+
+    public static final double STATE_TIMEOUT = 5.0;
+
+    public static final int CANCODER_ID = 0; // TODO: update id
+    public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; 
+
+    public static final TalonFXConfiguration CONFIG =
+      new TalonFXConfiguration()
+        .withCurrentLimits(
+          new CurrentLimitsConfigs()
+            .withSupplyCurrentLimit(60)
+            .withStatorCurrentLimit(60))
+        .withMotorOutput(
+          new MotorOutputConfigs()
+            .withInverted(InvertedValue.CounterClockwise_Positive)
+            .withNeutralMode(NeutralModeValue.Brake));
+
+    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);
+
+    public static final FeedbackConfigs FEEDBACK_CONFIGS =
+      new FeedbackConfigs()
+        .withFeedbackRemoteSensorID(CANCODER_ID)
+        .withFeedbackRotorOffset(CANCODER_ABSOLUTE_OFFSET)
+        .withSensorToMechanismRatio(CANCODER_TO_MECHANISM)
+        .withRotorToSensorRatio(MOTOR_TO_CANCODER)
+        .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder);
+
+    public static final MagnetSensorConfigs CANCODER_CONFIG =
+      new MagnetSensorConfigs()
+        .withMagnetOffset(1.0 * CANCODER_ABSOLUTE_OFFSET) 
+        .withAbsoluteSensorDiscontinuityPoint(0.5)
+        .withSensorDirection(SensorDirectionValue.Clockwise_Positive);
+  }
 }

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

@@ -23,6 +23,7 @@
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 import org.team5924.frc2026.subsystems.SuperShooter.ShooterState;
+import org.team5924.frc2026.subsystems.climb.Climb.ClimbState;
 import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState;
 import org.team5924.frc2026.subsystems.rollers.hopper.Hopper.HopperState;
 import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState;
@@ -66,6 +67,8 @@ public void resetPose(Pose2d pose) {
 
   /* ### Intake ### */
   @Getter @Setter private IntakeState intakeState = IntakeState.OFF;
+  /* ### Climb ### */
+  @Getter @Setter private ClimbState climbState = ClimbState.STOW;
 
   /* ### Hopper ### */
   @Getter @Setter private HopperState hopperState = HopperState.OFF;

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

@@ -0,0 +1,157 @@
+/*
+ * 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 java.util.function.DoubleSupplier;
+import lombok.Getter;
+import lombok.Setter;
+
+import org.littletonrobotics.junction.Logger;
+import org.team5924.frc2026.Constants;
+import org.team5924.frc2026.RobotState;
+import org.team5924.frc2026.subsystems.sensors.BeamBreakIO;
+import org.team5924.frc2026.subsystems.sensors.BeamBreakIOInputsAutoLogged;
+import org.team5924.frc2026.subsystems.turret.Turret.TurretState;
+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.EqualsUtil;
+import org.team5924.frc2026.util.LoggedTunableNumber;
+
+public class Climb extends SubsystemBase {
+
+  private final ClimbIO io;
+  private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged();
+
+  @Setter private double input;
+
+  public enum ClimbState { // TODO: Update climb level values - distance not rads!!!
+    STOW(new LoggedTunableNumber("Climb/Stow", 0)),
+    OFF(() -> 0.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
+    MANUAL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5));
+
+    // TODO: add distance to rads method
+
+    @Getter 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;
+
+  private double lastStateChange = 0.0;
+
+  // Climb Beam Break
+  private final BeamBreakIO beamBreakIO;
+  private final BeamBreakIOInputsAutoLogged beamBreakInputs = new BeamBreakIOInputsAutoLogged();
+
+  public Climb(ClimbIO io, BeamBreakIO beamBreakIO) {
+    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", "");
+    this.beamBreakIO = beamBreakIO;
+  }
+
+  @Override
+  public void periodic() {
+    io.periodicUpdates();
+    io.updateInputs(inputs);
+    beamBreakIO.updateInputs(beamBreakInputs);
+    Logger.processInputs("Climb", inputs);
+    Logger.processInputs("Climb/BeamBreak", beamBreakInputs);
+
+    Logger.recordOutput("Climb/GoalState", goalState.toString());
+    Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState().toString());
+    Logger.recordOutput("Climb/TargetRads", goalState.rads.getAsDouble());
+
+    climbMotorDisconnected.set(!inputs.climbMotorConnected);
+
+    handleCurrentState();
+
+    if (!inputs.climbMotorConnected && wasClimbMotorConnected) {
+      Elastic.sendNotification(climbMotorDisconnectedNotification);
+    }
+
+    wasClimbMotorConnected = inputs.climbMotorConnected;
+  }
+
+  public boolean isAtSetpoint() {
+    return RobotState.getTime() - lastStateChange < Constants.Climb.STATE_TIMEOUT
+        || EqualsUtil.epsilonEquals(
+            inputs.setpointRads, inputs.climbPositionRads, Constants.Climb.EPSILON_RADS);
+  }
+
+  private void handleCurrentState() {
+    switch (RobotState.getInstance().getClimbState()) {
+      case MOVING -> {
+        if (isAtSetpoint()) RobotState.getInstance().setClimbState(goalState);
+      }
+      case MANUAL -> handleManualState();
+      case OFF -> io.stop();
+      default -> io.setPosition(goalState.rads.getAsDouble());
+    }
+  }
+
+  private void handleManualState() {
+    if (!goalState.equals(TurretState.MANUAL)) return;
+
+    if (Math.abs(input) <= Constants.Climb.JOYSTICK_DEADZONE) {
+      io.runVolts(0);
+      return;
+    }
+
+    io.runVolts(ClimbState.MANUAL.getRads().getAsDouble() * input);
+  }
+
+  public void setGoalState(ClimbState goalState) {
+    this.goalState = goalState;
+    switch (goalState) {
+      case MANUAL:
+        RobotState.getInstance().setClimbState(ClimbState.MANUAL);
+        break;
+      case MOVING:
+        DriverStation.reportError(
+            "Climb: MOVING is an invalid goal state; it is a transition state!!", null);
+        break;
+      default:
+        RobotState.getInstance().setClimbState(goalState);
+        break;
+    }
+
+    lastStateChange = RobotState.getTime();
+  }
+}

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

@@ -0,0 +1,64 @@
+/*
+ * 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 climbPositionCancoder = 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;
+
+    public double setpointRads = 0.0;
+    public double acceleration = 0.0;
+
+    public boolean cancoderConnected = true;
+    public double cancoderAbsolutePosition = 0.0;
+    public double cancoderVelocity = 0.0;
+    public double cancoderSupplyVoltage = 0.0;
+    public double cancoderPositionRotations = 0.0;
+  }
+
+  /**
+   * Updates the inputs object with the latest data from hardware
+   *
+   * @param inputs Inputs to update
+   */
+  public default void updateInputs(ClimbIOInputs inputs) {}
+
+  /** Updates that are be called in climb periodic */
+  public default void periodicUpdates() {}
+
+  /**
+   * 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() {}
+}