Climber subsystem

unconfirmed subsytem - incomplete,no motors mapped

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,13 +7,13 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2026KPopRobotHunters";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 23;
+  public static final int GIT_REVISION = 24;
-  public static final String GIT_SHA = "8bda61b9837d7450a2fb4650bf02ffb1f4f06213";
+  public static final String GIT_SHA = "5d381731689602f5fac161f1552170003ef30c14";
-  public static final String GIT_DATE = "2026-01-29 16:59:53 MST";
+  public static final String GIT_DATE = "2026-01-29 18:07:19 MST";
   public static final String GIT_BRANCH = "Subsystem-Boilerplate";
-  public static final String BUILD_DATE = "2026-01-29 17:04:31 MST";
+  public static final String BUILD_DATE = "2026-01-29 19:26:44 MST";
-  public static final long BUILD_UNIX_TIME = 1769731471423L;
+  public static final long BUILD_UNIX_TIME = 1769740004409L;
-  public static final int DIRTY = 0;
+  public static final int DIRTY = 1;
 
   private BuildConstants(){}
 }

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

@@ -0,0 +1,79 @@
+package frc4388.robot.subsystems.climber;
+
+import static edu.wpi.first.units.Units.Rotation;
+
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.utility.status.FaultReporter;
+
+public class Climber extends SubsystemBase {
+    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 {
+    //     Up,
+    //     Down,
+    // }
+
+    // public void setMode(IntakeMode mode) {
+    //     switch (mode) {
+    //         case Up:
+    //             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 enum FieldZone {
+    //     // The robot should aim at the hub
+    //     InShootZone,
+    //     // The robot should aim towards the wall
+    //     AimAtWall,
+        
+    // }
+
+    // // Calculate what should be done based off of the position of the robot
+    // // TODO: Implement field zones
+    // public FieldZone getTarget(Pose2d position) {
+    //     return FieldZone.InShootZone;
+    // }
+
+    @Override
+    public void periodic() {
+        
+        
+
+        // FaultReporter.register(this); // TODO Implement fault reporter
+
+
+        Logger.processInputs("Climber", state);
+
+        Pose2d pose = m_swervePoseSupplier.get();
+        Angle robotRot = pose.getRotation().getMeasure();
+
+        io.updateInputs(state);
+
+    }
+}
+
+

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

@@ -0,0 +1,74 @@
+package frc4388.robot.subsystems.climber;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.Slot1Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
+import frc4388.utility.status.CanDevice;
+
+public class ClimberConstants {
+    // Motor conversions
+
+    public static final double CLIMBER_MOTOR_GEAR_RATIO = 1.;
+
+    //IDs
+
+
+    // 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 Distance CLIMBER_LIMIT_UPPER = Inches.of(0);
+
+
+    public static final Slot0Configs CLIMBER_PID = new Slot0Configs()
+        .withKP(2.0)
+        .withKI(0.0)
+        .withKD(10.0);
+
+    // 0 is paralell to the ground, 90 is directly up
+    // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
+    // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
+    
+    // Motor configs
+    public static final TalonFXConfiguration CLIMBER_MOTOR_CONFIG = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(40) // TODO: tune???
+                .withStatorCurrentLimitEnable(true)
+            ).withMotorOutput(
+                new MotorOutputConfigs()
+                    .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

@@ -0,0 +1,43 @@
+package frc4388.robot.subsystems.climber;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.InchesPerSecond;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Inches;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.units.CurrentUnit;
+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.Distance;
+import edu.wpi.first.units.measure.LinearVelocity;
+
+public interface ClimberIO {
+    @AutoLog
+    public class ClimberState {
+        Distance climberDistance = Inches.of(0);
+        Distance climberTargetDistance = Inches.of(0);
+        Current climberMotorCurrent = Amps.of(0);
+
+        // 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 updateInputs(ClimberState state) {}
+}

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

@@ -0,0 +1,78 @@
+package frc4388.robot.subsystems.climber;
+
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.InchesPerSecond;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.units.measure.*;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.subsystems.intake.IntakeConstants;
+import frc4388.utility.configurable.ConfigurableDouble;
+
+public class ClimberReal implements ClimberIO {
+
+
+    TalonFX m_climberMotor;
+
+    public ClimberReal(
+
+        TalonFX climberMotor
+    ) {
+        // m_angleMotor = angleMotor;
+        // m_pitchMotor = pitchMotor;
+        m_climberMotor = climberMotor;
+        // Apply the configs
+        m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_PID);
+        m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_MOTOR_CONFIG);
+
+    }
+
+    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
+        
+        Distance boundedDistance = clampDistance(distance, ClimberConstants.CLIMBER_LIMIT_LOWER, ClimberConstants.CLIMBER_LIMIT_UPPER);
+        // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
+        double motorTargetDistance = boundedDistance.in(Inches) / ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO;
+        PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetDistance);
+        m_climberMotor.setControl(posRequest);
+    }
+
+    @Override
+    public void updateInputs(ClimberState state) {
+        double motorRotations = m_climberMotor.getPosition().getValue().in(Rotations);
+        double linearInches = motorRotations * ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO;
+        state.climberDistance = Inches.of(linearInches);
+        state.climberMotorCurrent = m_climberMotor.getStatorCurrent(false).getValue();
+
+        // 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();
+
+
+    }
+}