healthcheck, elevator algae l2, climb pos and no safety, l2/3 coral eject slower

Author: ds12a

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

@@ -52,6 +52,7 @@
 import frc.robot.commands.robotState.SetScoreSideRightCommand;
 import frc.robot.commands.robotState.SetScoringLevelCommand;
 import frc.robot.commands.robotState.StopAllAxisCommand;
+import frc.robot.commands.robotState.StopOpenLoopCommand;
 import frc.robot.commands.robotState.StowCommand;
 import frc.robot.commands.robotState.ToggleAlgaeHeightCommand;
 import frc.robot.commands.robotState.ToggleAutoPlacingCommand;
@@ -619,6 +620,7 @@ private void configurePitDashboard() {
         .add(
             "Health Check",
             new DepthChargeHealthCheckCommand(
+                robotStateSubsystem,
                 swerve,
                 driveSubsystem,
                 funnelIO,
@@ -633,6 +635,13 @@ private void configurePitDashboard() {
                 biscuitSubsystem))
         .withPosition(6, 1)
         .withSize(1, 1);
+
+    Shuffleboard.getTab("Pit")
+        .add(
+            "Stop Open Loop",
+            new StopOpenLoopCommand(robotStateSubsystem, coralSubsystem, funnelIO, algaeIO))
+        .withPosition(7, 1)
+        .withSize(1, 1);
   }
 
   public void configTestDash() {

src/main/java/frc/robot/commands/drive/SpinUpWheelsCommand.java

@@ -1,17 +1,24 @@
 package frc.robot.commands.drive;
 
-import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.drive.DriveSubsystem;
 
-public class SpinUpWheelsCommand extends InstantCommand {
+public class SpinUpWheelsCommand extends Command {
   private DriveSubsystem driveSubsystem;
 
   public SpinUpWheelsCommand(DriveSubsystem driveSubsystem) {
     this.driveSubsystem = driveSubsystem;
+
+    addRequirements(driveSubsystem);
   }
 
   @Override
   public void initialize() {
     driveSubsystem.setAzimuthVel(0.2);
   }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
 }

src/main/java/frc/robot/commands/robotState/DepthChargeHealthCheckCommand.java

@@ -1,5 +1,6 @@
 package frc.robot.commands.robotState;
 
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.algae.AlgaeIOFX;
 import frc.robot.subsystems.algae.AlgaeSubsystem;
@@ -13,6 +14,8 @@
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.funnel.FunnelIOFXS;
 import frc.robot.subsystems.funnel.FunnelSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
 import java.util.List;
 import org.strykeforce.healthcheck.IOHealthCheckCommand;
 
@@ -31,6 +34,7 @@ public class DepthChargeHealthCheckCommand extends SequentialCommandGroup {
   private BiscuitSubsystem biscuitSubsystem;
 
   public DepthChargeHealthCheckCommand(
+      RobotStateSubsystem robotStateSubsystem,
       SwerveFXS swerve,
       DriveSubsystem driveSubsystem,
       FunnelIOFXS funnelIOFXS,
@@ -59,6 +63,10 @@ public DepthChargeHealthCheckCommand(
 
     addCommands(
         new ResetCaseHealthCheckCommand(),
+        new InstantCommand(() -> robotStateSubsystem.setState(RobotStates.IDLE, false)),
+        new InstantCommand(() -> coralSubsystem.healthCheck()),
+        new InstantCommand(() -> funnelIOFXS.setPct(0)),
+        new InstantCommand(() -> algaeIOFXS.setPct(0)),
         new IOHealthCheckCommand(
             List.of(
                 driveSubsystem,
@@ -73,6 +81,12 @@ public DepthChargeHealthCheckCommand(
             algaeIOFXS,
             elevatorIOFX,
             biscuitIOFXS),
-        new LockWheelsCommand(driveSubsystem));
+        new LockWheelsCommand(driveSubsystem),
+        new StowCommand(
+            robotStateSubsystem,
+            elevatorSubsystem,
+            coralSubsystem,
+            biscuitSubsystem,
+            algaeSubsystem));
   }
 }

src/main/java/frc/robot/commands/robotState/StopOpenLoopCommand.java

@@ -0,0 +1,24 @@
+package frc.robot.commands.robotState;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.algae.AlgaeIOFX;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.funnel.FunnelIOFXS;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
+
+public class StopOpenLoopCommand extends SequentialCommandGroup {
+  public StopOpenLoopCommand(
+      RobotStateSubsystem robotStateSubsystem,
+      CoralSubsystem coralSubsystem,
+      FunnelIOFXS funnelIOFXS,
+      AlgaeIOFX algaeIOFXS) {
+    addCommands(
+        new ResetCaseHealthCheckCommand(),
+        new InstantCommand(() -> robotStateSubsystem.setState(RobotStates.IDLE, false)),
+        new InstantCommand(() -> coralSubsystem.healthCheck()),
+        new InstantCommand(() -> funnelIOFXS.setPct(0)),
+        new InstantCommand(() -> algaeIOFXS.setPct(0)));
+  }
+}

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

@@ -29,7 +29,7 @@ public class ClimbConstants {
   public static int kCageAlignedDIOId = 11;
 
   public static final double kPivotArmCloseEnough = 0.01; // fixme
-  public static final double kArmMaxFwd = 0;
+  public static final double kArmMaxFwd = 0.01;
   public static final double kArmMaxRev = -0.29;
   public static final Angle kArmZeroTicks = Degrees.of(1530);
 

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

@@ -42,7 +42,7 @@ public class ElevatorConstants {
   // Idle
 
   // Algae removal
-  public static final Angle kL2AlgaeSetpoint = Rotations.of(6.308);
+  public static final Angle kL2AlgaeSetpoint = Rotations.of(5.826); // was 6.308
   public static final Angle kL3AlgaeSetpoint = Rotations.of(17.513);
 
   public static final Angle kL2AlgaeRemovalSetpoint = kL2AlgaeSetpoint;

src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java

@@ -23,7 +23,7 @@ public class AlgaeIOFX implements AlgaeIO, Checkable {
 
   @HealthCheck
   @Timed(
-      percentOutput = {-0.1, -1},
+      percentOutput = {0.5, 0.04, -1},
       duration = 2)
   private TalonFXS talonFXS;
 

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

@@ -32,8 +32,8 @@ public class BiscuitIOFXS implements BiscuitIO, Checkable {
 
   @HealthCheck
   @Position(
-      percentOutput = {-0.1, 0.1},
-      encoderChange = 40)
+      percentOutput = {0.05, -0.10},
+      encoderChange = 20)
   private TalonFXS talon;
 
   private final Angle sensorInitial;
@@ -48,7 +48,7 @@ public class BiscuitIOFXS implements BiscuitIO, Checkable {
   private Alert rangeAlert = new Alert("Biscuit overextended! Shuting down!", AlertType.kError);
   private boolean lastHadAlgae = false;
 
-  TalonFXSConfigurator configurator;
+  private TalonFXSConfigurator configurator;
   private MotionMagicDutyCycle positionRequest =
       new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0);
 
@@ -133,7 +133,8 @@ public String getName() {
 
   @BeforeHealthCheck
   @AfterHealthCheck
-  public void healthCheckPos() {
+  public boolean healthCheckPos() {
     setPosition(Rotations.of(0), false);
+    return Math.abs(position.refresh().getValueAsDouble()) < BiscuitConstants.kCloseEnough;
   }
 }

src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java

@@ -5,6 +5,7 @@
 import edu.wpi.first.units.measure.AngularVelocity;
 import frc.robot.constants.CoralConstants;
 import frc.robot.standards.ClosedLoopSpeedSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
 import java.util.Set;
 import org.littletonrobotics.junction.Logger;
 import org.slf4j.LoggerFactory;
@@ -99,10 +100,14 @@ public void intake() {
     setState(CoralState.INTAKING);
   }
 
-  public void eject() {
+  public void eject(ScoringLevel level) {
     io.enableFwdLimitSwitch(false);
     // setSpeed(CoralConstants.kEjectingSpeed);
-    setPct(1);
+
+    switch (level) {
+      case L1, L2, L3 -> setPct(0.8);
+      case L4 -> setPct(1);
+    }
     setState(CoralState.EJECTING);
   }
 
@@ -152,6 +157,11 @@ public Set<Measure> getMeasures() {
     return Set.of(new Measure("State", () -> curState.ordinal()));
   }
 
+  public void healthCheck() {
+    setState(CoralState.IDLE);
+    setPct(0);
+  }
+
   public enum CoralState {
     IDLE,
     HAS_CORAL,

src/main/java/frc/robot/subsystems/elevator/ElevatorIOFX.java

@@ -1,5 +1,7 @@
 package frc.robot.subsystems.elevator;
 
+import static edu.wpi.first.units.Units.Rotations;
+
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
@@ -28,7 +30,7 @@ public class ElevatorIOFX implements ElevatorIO, Checkable {
 
   @HealthCheck
   @Position(
-      percentOutput = {-0.1, 0.1},
+      percentOutput = {-0.05, 0.1},
       encoderChange = 10)
   private TalonFX talonFxFront;
 
@@ -118,8 +120,12 @@ public void zero() {
 
   @BeforeHealthCheck
   @AfterHealthCheck
-  public void healthCheckPos() {
+  public boolean healthCheckPos() {
     setPosition(ElevatorConstants.kHealthCheck);
+    return Math.abs(
+            currPosition.refresh().getValueAsDouble()
+                - ElevatorConstants.kHealthCheck.in(Rotations))
+        < ElevatorConstants.kCloseEnoughRotations;
   }
 
   @Override