Merge pull request #11 from IgniteRobotics/feature/climber

Climber setup

Author: Orcasphynx

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

@@ -0,0 +1,46 @@
+package frc.robot.subsystems.climber;
+
+import static edu.wpi.first.units.Units.Amp;
+import static edu.wpi.first.units.Units.Rotations;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Current;
+
+final class ClimberConstants {
+
+  private ClimberConstants() {}
+
+  // TODO: Change to actual port
+  protected static final int CLIMB_MOTOR_ID = 10;
+
+  // TODO: Tune motor
+  protected static final Angle ALLOWABLE_CLIMB_ERROR = Rotations.of(0.1);
+  protected static final Angle CLIMB_FORWARD_LIMIT = Rotations.of(100);
+  protected static final Angle CLIMB_REVERSE_LIMIT = Rotations.of(0);
+  protected static final double CLIMB_KS = 0;
+  protected static final double CLIMB_KP = 0;
+  protected static final double CLIMB_KD = 0;
+
+  protected static Slot0Configs createClimbMotorSlot0Configs() {
+    Slot0Configs slot = new Slot0Configs();
+    slot.kS = CLIMB_KS;
+    slot.kP = CLIMB_KP;
+    slot.kD = CLIMB_KD;
+    return slot;
+  }
+
+  protected static SoftwareLimitSwitchConfigs createClimbSoftwareLimitSwitchConfigs() {
+    SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs();
+    configs.ForwardSoftLimitEnable = false;
+    configs.ReverseSoftLimitEnable = false;
+    configs.ForwardSoftLimitThreshold = CLIMB_FORWARD_LIMIT.in(Rotations);
+    configs.ReverseSoftLimitThreshold = CLIMB_REVERSE_LIMIT.in(Rotations);
+    return configs;
+  }
+
+  protected static final DutyCycleOut SAFE_HOMING_EFFORT = new DutyCycleOut(-0.2);
+  protected static final Current SAFE_STATOR_LIMIT = Amp.of(0.8);
+}

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

@@ -0,0 +1,9 @@
+package frc.robot.subsystems.climber;
+
+import frc.robot.preferences.DoublePreference;
+
+final class ClimberPreferences {
+
+  protected static DoublePreference pullPosition =
+      new DoublePreference("Climber/Pull Down (Rotations)", 3); // in rotations
+}

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

@@ -0,0 +1,62 @@
+package frc.robot.subsystems.climber;
+
+import static edu.wpi.first.units.Units.Amp;
+import static edu.wpi.first.units.Units.Rotations;
+
+import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class ClimberSubsystem extends SubsystemBase {
+  private final TalonFX climbMotor;
+
+  private Angle climbTarget; // Rotations
+  private PositionTorqueCurrentFOC climbControl;
+
+  public ClimberSubsystem() {
+    climbMotor = new TalonFX(ClimberConstants.CLIMB_MOTOR_ID);
+
+    climbMotor.getConfigurator().apply(ClimberConstants.createClimbMotorSlot0Configs());
+    climbMotor.getConfigurator().apply(ClimberConstants.createClimbSoftwareLimitSwitchConfigs());
+    climbMotor.setPosition(0);
+    climbTarget = Rotations.of(0);
+    climbControl = new PositionTorqueCurrentFOC(0);
+  }
+
+  @Override
+  public void periodic() {
+    climbMotor.setControl(climbControl.withPosition(climbTarget));
+  }
+
+  private boolean atClimbSetpoint() {
+    return Math.abs(climbMotor.getPosition().getValueAsDouble() - climbTarget.in(Rotations))
+        < ClimberConstants.ALLOWABLE_CLIMB_ERROR.in(Rotations);
+  }
+
+  public Command climbToPosition(Angle position) {
+    return runOnce(() -> climbTarget = position)
+        .andThen(Commands.waitUntil(() -> atClimbSetpoint()));
+  }
+
+  public Command resetClimb() {
+    return climbToPosition(Rotations.of(0));
+  }
+
+  public Command climbToPreference() {
+    return climbToPosition(Rotations.of(ClimberPreferences.pullPosition.getValue()));
+  }
+
+  public Command homeClimber() {
+    return runEnd(
+            () -> climbMotor.set(ClimberConstants.SAFE_HOMING_EFFORT.Output),
+            () -> climbMotor.setPosition(0))
+        .until(
+            () -> {
+              return climbMotor.getStatorCurrent().getValueAsDouble()
+                  > ClimberConstants.SAFE_STATOR_LIMIT.in(Amp);
+            });
+  }
+}