add in initial climb test

Author: mwitcpalek

src/main/java/frc/robot/RobotContainer.java

@@ -60,6 +60,8 @@
 import frc.robot.subsystems.battMon.BattMonSubsystem;
 import frc.robot.subsystems.biscuit.BiscuitIOFX;
 import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+import frc.robot.subsystems.climb.ClimbIO;
+import frc.robot.subsystems.climb.ClimbIOServoFX;
 import frc.robot.subsystems.climb.ClimbSubsystem;
 import frc.robot.subsystems.coral.CoralIO;
 import frc.robot.subsystems.coral.CoralIOFX;
@@ -93,6 +95,7 @@ public class RobotContainer {
   private final BiscuitIOFX biscuitIO;
   private final BiscuitSubsystem biscuitSubsystem;
 
+  private final ClimbIO climbIO;
   private final ClimbSubsystem climbSubsystem;
 
   private final CoralIO coralIO;
@@ -132,7 +135,8 @@ public RobotContainer() {
     biscuitIO = new BiscuitIOFX();
     biscuitSubsystem = new BiscuitSubsystem(biscuitIO);
 
-    climbSubsystem = new ClimbSubsystem();
+    climbIO = new ClimbIOServoFX();
+    climbSubsystem = new ClimbSubsystem(climbIO);
 
     coralIO = new CoralIOFX();
     coralSubsystem = new CoralSubsystem(coralIO);
@@ -186,6 +190,7 @@ private void configureTelemetry() {
     funnelSubsystem.registerWith(telemetryService);
     biscuitSubsystem.registerWith(telemetryService);
     ledSubsystem.registerWith(telemetryService);
+    climbSubsystem.registerWith(telemetryService);
     telemetryService.start();
   }
 
@@ -264,6 +269,10 @@ private void configureDriverBindings() {
         .onFalse(
             new FloorAlgaeCommand(
                 robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
+
+    // climb
+    new JoystickButton(driveJoystick, Button.SWA.id)
+        .onTrue(new InstantCommand(() -> robotStateSubsystem.toClimb()));
   }
 
   private void configureOperatorBindings() {
@@ -311,6 +320,10 @@ private void configureOperatorBindings() {
         .onTrue(new SetScoreSideRightCommand(robotStateSubsystem));
     new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
         .onTrue(new setScoreSideLeftCommand(robotStateSubsystem));
+
+    // Prep Climb
+    new JoystickButton(xboxController, XboxController.Button.kStart.value)
+        .onTrue(new InstantCommand(() -> robotStateSubsystem.toPrepClimb()));
   }
 
   private void configureTestOperatorBindings() {
@@ -473,6 +486,15 @@ public void configurePitDashboard() {
         .withPosition(1, 0)
         .withSize(1, 1);
 
+    Shuffleboard.getTab("Pit")
+        .add("Prep Climb", new InstantCommand(() -> robotStateSubsystem.toPrepClimb()))
+        .withPosition(3, 0)
+        .withSize(1, 1);
+    Shuffleboard.getTab("Pit")
+        .add("Climb", new InstantCommand(() -> robotStateSubsystem.toClimb()))
+        .withPosition(4, 0)
+        .withSize(1, 1);
+
     // Shuffleboard.getTab("Pit")
     //     .add(
     //         "Start Auton",

src/main/java/frc/robot/constants/ClimbConstants.java

@@ -1,14 +1,17 @@
 package frc.robot.constants;
 
 import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Rotations;
 
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
 import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
 import com.ctre.phoenix6.signals.GravityTypeValue;
@@ -26,22 +29,24 @@ public class ClimbConstants {
   public static int kRatchetServoId = 2;
   public static int kCageAlignedDIOId = 11;
 
-  public static final Angle kPivotArmCloseEnough =
-      Degrees.of(5.0); // all of these arbitrary numbers
-  public static final Angle kArmMaxFwd = Degrees.of(100);
-  public static final Angle kArmMaxRev = Degrees.of(-100);
+  public static final double kPivotArmCloseEnough = 0.01; // FIXME
+  public static final double kArmMaxFwd = 0.260;
+  public static final double kArmMaxRev = 0.03;
   public static final Angle kArmZeroTicks = Degrees.of(1530);
 
   // Deploy Servo
-  public static final double kPinDeployedPosition = 1.0;
-  public static final double kPinRetractedPosition = 0.0;
+  public static final double kPinDeployedPosition = 0.75;
+  public static final double kPinRetractedPosition = 0.18;
 
   // Ratchet Servo
-  public static final double kRatchetEngatedPos = 1.0;
-  public static final double kRatchetDisengagedPos = 0.0;
+  public static final double kRatchetEngagedPos = 0.0;
+  public static final double kRatchetDisengagedPos = 1.0;
 
   // Climb positions
-  public static final Angle kClimbCagePos = Degrees.of(3);
+  public static final Angle kClimbCagePos = Rotations.of(4); // fixme
+  public static final Double kClimbRatchedEngage = 0.1;
+  public static final double kFullyClimbed = 0.260;
+  public static final double kClimbOpenLoopSpeed = 4.0;
 
   public static TalonFXConfiguration getPivotArmFxConfig() {
     TalonFXConfiguration armFxConfig = new TalonFXConfiguration();
@@ -50,10 +55,9 @@ public static TalonFXConfiguration getPivotArmFxConfig() {
         new CurrentLimitsConfigs()
             .withStatorCurrentLimit(10)
             .withStatorCurrentLimitEnable(false)
-            .withStatorCurrentLimit(20)
-            .withSupplyCurrentLimit(10)
-            .withSupplyCurrentLowerLimit(8)
-            .withSupplyCurrentLowerTime(0.02)
+            .withSupplyCurrentLimit(30)
+            .withSupplyCurrentLowerLimit(30)
+            .withSupplyCurrentLowerTime(1)
             .withSupplyCurrentLimitEnable(true);
     armFxConfig.CurrentLimits = current;
 
@@ -101,9 +105,15 @@ public static TalonFXConfiguration getPivotArmFxConfig() {
     MotorOutputConfigs motorOut =
         new MotorOutputConfigs()
             .withDutyCycleNeutralDeadband(0.01)
-            .withNeutralMode(NeutralModeValue.Coast);
+            .withNeutralMode(NeutralModeValue.Brake);
     armFxConfig.MotorOutput = motorOut;
 
+    FeedbackConfigs feedbackConfigs =
+        new FeedbackConfigs()
+            .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
+            .withFeedbackRemoteSensorID(kCANcoderId);
+    armFxConfig.Feedback = feedbackConfigs;
+
     return armFxConfig;
   }
 

src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java

@@ -55,9 +55,9 @@ public BiscuitIOFX() {
     velocity = talon.getVelocity();
     position = talon.getPosition();
     rawQuadrature = talon.getRawQuadraturePosition();
-    rawQuadrature.setUpdateFrequency(200);
+    rawQuadrature.setUpdateFrequency(20);
     rawPulseWidth = talon.getRawPulseWidthPosition();
-    rawPulseWidth.setUpdateFrequency(20);
+    rawPulseWidth.setUpdateFrequency(200);
     zero();
   }
 

src/main/java/frc/robot/subsystems/climb/ClimbIO.java

@@ -1,7 +1,5 @@
 package frc.robot.subsystems.climb;
 
-import static edu.wpi.first.units.Units.Rotations;
-
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import edu.wpi.first.units.measure.Angle;
 import org.littletonrobotics.junction.AutoLog;
@@ -11,7 +9,7 @@ public interface ClimbIO {
 
   @AutoLog
   static class ClimbIOInputs {
-    public Angle position = Rotations.of(0.0);
+    public double position = 0.0;
     public double velocity = 0.0;
     public double ratchetServoPosition = 0.0;
     public double pinServoPosition = 0.0;
@@ -34,4 +32,8 @@ public default void registerWith(TelemetryService telemetryService) {}
   public default void setSoftLimitsEnabled(boolean enable) {}
 
   public default void setCurrentLimit(CurrentLimitsConfigs config) {}
+
+  public default void setPercent(double percent) {}
+
+  public default void setCoastMode(boolean coast) {}
 }

src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java

@@ -7,19 +7,23 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfigurator;
 import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.Servo;
 import frc.robot.constants.ClimbConstants;
 import frc.robot.subsystems.climb.ClimbIO.ClimbIOInputs;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
 
 public class ClimbIOServoFX implements ClimbIO {
   // will prob need healthchecks eventually
   private Logger logger;
   private TalonFX talonFxPivotArmFront;
-  private TalonFX talonFxPivotArmBack;
+  private CANcoder canCoder;
   private Servo ratchetServo;
   private Servo pinServo;
 
@@ -28,26 +32,24 @@ public class ClimbIOServoFX implements ClimbIO {
   private Angle pivotArmSetpoint;
 
   TalonFXConfigurator configuratorFront;
-  TalonFXConfigurator configuratorBack;
   private MotionMagicDutyCycle positionRequestMain =
       new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
+  private VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(false);
 
   StatusSignal<Angle> currPosition;
 
   public ClimbIOServoFX() {
     logger = LoggerFactory.getLogger(this.getClass());
     talonFxPivotArmFront = new TalonFX(ClimbConstants.kPivotArmFrontFxId);
     ratchetServo = new Servo(ClimbConstants.kRatchetServoId);
+    canCoder = new CANcoder(ClimbConstants.kCANcoderId);
     pinServo = new Servo(ClimbConstants.kDeployServoId);
 
     absPivotArmFrontSensorInitial =
         talonFxPivotArmFront.getPosition().getValue(); // only for logging
     configuratorFront = talonFxPivotArmFront.getConfigurator();
-    configuratorBack = talonFxPivotArmBack.getConfigurator();
     configuratorFront.apply(new TalonFXConfiguration());
     configuratorFront.apply(ClimbConstants.getPivotArmFxConfig());
-    configuratorBack.apply(new TalonFXConfiguration());
-    configuratorBack.apply(ClimbConstants.getPivotArmFxConfig());
 
     currPosition = talonFxPivotArmFront.getPosition();
   }
@@ -81,7 +83,7 @@ public void setPinServoPosition(double position) {
 
   @Override
   public void updateInputs(ClimbIOInputs inputs) {
-    inputs.position = currPosition.refresh().getValue();
+    inputs.position = currPosition.refresh().getValueAsDouble();
     inputs.ratchetServoPosition = ratchetServo.getPosition();
     inputs.pinServoPosition = pinServo.getPosition();
   }
@@ -99,7 +101,6 @@ public void zero() {
 
     );
     */
-    talonFxPivotArmBack.setPosition(0.0);
     talonFxPivotArmFront.setPosition(0.0);
   }
 
@@ -117,8 +118,26 @@ public void setSoftLimitsEnabled(boolean enable) {
   @Override
   public void setCurrentLimit(CurrentLimitsConfigs config) {
     configuratorFront.apply(config);
-    configuratorBack.apply(config);
+  }
+
+  @Override
+  public void setPercent(double percent) {
+    talonFxPivotArmFront.setControl(voltageRequest.withOutput(percent));
+  }
+
+  @Override
+  public void setCoastMode(boolean coast) {
+    configuratorFront.apply(
+        ClimbConstants.getPivotArmFxConfig()
+            .MotorOutput
+            .withNeutralMode(coast ? NeutralModeValue.Coast : NeutralModeValue.Brake));
   }
 
   public void goToZero() {}
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    telemetryService.register(talonFxPivotArmFront, false);
+    telemetryService.register(canCoder);
+  }
 }