Merge pull request #83 from strykeforce/healthCheck

mwitcpalek · web-flow · commit 5b447d802f7b · 2023-03-08T23:07:28.000-05:00
Initial Healthcheck Implementation
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -44,6 +44,7 @@
 import frc.robot.commands.robotState.SetGamePieceCommand;
 import frc.robot.commands.robotState.SetLevelAndColCommandGroup;
 import frc.robot.commands.robotState.ShelfPickupCommand;
+import frc.robot.commands.robotState.ShuffleBoardHealthCheckCommandGroup;
 import frc.robot.commands.robotState.StowRobotCommand;
 import frc.robot.commands.robotState.ToggleIntakeCommand;
 import frc.robot.commands.shoulder.ShoulderSpeedCommand;
@@ -587,6 +588,20 @@ private void configurePitDashboard() {
     gamePieceCommands
         .add("Set cube", new SetGamePieceCommand(robotStateSubsystem, GamePiece.CUBE))
         .withPosition(0, 1);
+
+    ShuffleboardLayout HealthCheck =
+        pitTab.getLayout("HealthCheck", BuiltInLayouts.kGrid).withPosition(1, 0).withSize(1, 1);
+    HealthCheck.add(
+            "HealthCheck",
+            new ShuffleBoardHealthCheckCommandGroup(
+                elbowSubsystem,
+                shoulderSubsystem,
+                elevatorSubsystem,
+                handSubsystem,
+                driveSubsystem,
+                intakeSubsystem,
+                armSubsystem))
+        .withPosition(0, 0);
   }
 
   public void setAllianceColor(Alliance alliance) {
diff --git a/src/main/java/frc/robot/commands/robotState/ShuffleBoardHealthCheckCommandGroup.java b/src/main/java/frc/robot/commands/robotState/ShuffleBoardHealthCheckCommandGroup.java
@@ -0,0 +1,35 @@
+package frc.robot.commands.robotState;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.drive.LockZeroCommand;
+import frc.robot.subsystems.ArmSubsystem;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.ElbowSubsystem;
+import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.HandSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.ShoulderSubsystem;
+import org.strykeforce.healthcheck.HealthCheckCommand;
+
+public class ShuffleBoardHealthCheckCommandGroup extends SequentialCommandGroup {
+  public ShuffleBoardHealthCheckCommandGroup(
+      ElbowSubsystem elbowSubsystem,
+      ShoulderSubsystem shoulderSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      HandSubsystem handSubsystem,
+      DriveSubsystem driveSubsystem,
+      IntakeSubsystem intakeSubsystem,
+      ArmSubsystem armSubsystem) {
+    addCommands(
+        new toggleHealthBoolean(armSubsystem),
+        new HealthCheckCommand(
+            driveSubsystem,
+            handSubsystem,
+            elevatorSubsystem,
+            elbowSubsystem,
+            shoulderSubsystem,
+            intakeSubsystem),
+        new LockZeroCommand(driveSubsystem),
+        new toggleHealthBoolean(armSubsystem));
+  }
+}
diff --git a/src/main/java/frc/robot/commands/robotState/toggleHealthBoolean.java b/src/main/java/frc/robot/commands/robotState/toggleHealthBoolean.java
@@ -0,0 +1,17 @@
+package frc.robot.commands.robotState;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.ArmSubsystem;
+
+public class toggleHealthBoolean extends InstantCommand {
+  ArmSubsystem armSubsystem;
+
+  public toggleHealthBoolean(ArmSubsystem armSubsystem) {
+    this.armSubsystem = armSubsystem;
+  }
+
+  @Override
+  public void initialize() {
+    armSubsystem.toggleHealthBoolean();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -30,6 +30,7 @@ public class ArmSubsystem extends MeasurableSubsystem {
   private boolean hasElbowZeroed = true;
   private boolean isElbowReinforced = true;
   private boolean doReinforceElevator = true;
+  private boolean isHealthChecking = false;
 
   public ArmSubsystem(
       ShoulderSubsystem shoulderSubsystem,
@@ -408,16 +409,23 @@ public boolean isFastStowing() {
     return shouldFastStowArm;
   }
 
+  public void toggleHealthBoolean() {
+    isHealthChecking = !isHealthChecking;
+  }
+
   @Override
   public void periodic() {
     HandRegion currHandRegion = getHandRegion();
 
-    shoulderSubsystem.setSoftLimits(
-        currHandRegion.minTicksShoulder, currHandRegion.maxTicksShoulder);
-    elevatorSubsystem.setSoftLimits(
-        currHandRegion.minTicksElevator, currHandRegion.maxTicksElevator);
-    elbowSubsystem.setSoftLimits(currHandRegion.minTicksElbow, currHandRegion.maxTicksElbow);
-
+    if (!isHealthChecking) {
+      shoulderSubsystem.setSoftLimits(
+          currHandRegion.minTicksShoulder, currHandRegion.maxTicksShoulder);
+      elevatorSubsystem.setSoftLimits(
+          currHandRegion.minTicksElevator, currHandRegion.maxTicksElevator);
+      elbowSubsystem.setSoftLimits(currHandRegion.minTicksElbow, currHandRegion.maxTicksElbow);
+    } else {
+      shoulderSubsystem.setSoftLimits(-500.0, 3_000.0);
+    }
     switch (currState) {
       case STOW:
         if (!hasElbowZeroed
diff --git a/src/main/java/frc/robot/subsystems/ElbowSubsystem.java b/src/main/java/frc/robot/subsystems/ElbowSubsystem.java
@@ -10,13 +10,23 @@
 import java.util.Set;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.healthcheck.AfterHealthCheck;
+import org.strykeforce.healthcheck.BeforeHealthCheck;
+import org.strykeforce.healthcheck.HealthCheck;
+import org.strykeforce.healthcheck.Position;
 import org.strykeforce.telemetry.TelemetryService;
 import org.strykeforce.telemetry.measurable.CanifierMeasurable;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
 
 public class ElbowSubsystem extends MeasurableSubsystem implements ArmComponent {
+
+  @HealthCheck
+  @Position(
+      percentOutput = {0.1, -0.1},
+      encoderChange = 5000)
   private TalonFX elbowFalcon;
+
   private double setPointTicks = 0;
   private CANifier remoteEncoder;
   private Logger logger = LoggerFactory.getLogger(ElbowSubsystem.class);
@@ -40,6 +50,18 @@ public ElbowSubsystem(Constants constants) {
     zeroElbow();
   }
 
+  @BeforeHealthCheck
+  public boolean goToZero() {
+    setPos(0.0);
+    return isFinished();
+  }
+
+  @AfterHealthCheck
+  public boolean returnToZero() {
+    setPos(0.0);
+    return isFinished();
+  }
+
   private int getPulseWidthFor(PWMChannel channel) {
     double[] pulseWidthandPeriod = new double[2];
     remoteEncoder.getPWMInput(channel, pulseWidthandPeriod);
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -8,14 +8,28 @@
 import java.util.Set;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.healthcheck.AfterHealthCheck;
+import org.strykeforce.healthcheck.BeforeHealthCheck;
+import org.strykeforce.healthcheck.Follow;
+import org.strykeforce.healthcheck.HealthCheck;
+import org.strykeforce.healthcheck.Position;
 import org.strykeforce.telemetry.TelemetryService;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
 
 public class ElevatorSubsystem extends MeasurableSubsystem implements ArmComponent {
   private static final Logger logger = LoggerFactory.getLogger(ElevatorSubsystem.class);
+
+  @HealthCheck
+  @Position(
+      percentOutput = {-0.2, 0.2},
+      encoderChange = 3_000)
   private TalonFX leftMainFalcon;
+
+  @HealthCheck
+  @Follow(leader = ElevatorConstants.kLeftMainId)
   private TalonFX rightFollowFalcon;
+
   private double desiredPosition;
   private ElevatorState elevatorState;
   private boolean hasZeroed = false;
@@ -45,6 +59,18 @@ public ElevatorSubsystem() {
     elevatorState = ElevatorState.IDLE;
   }
 
+  @BeforeHealthCheck
+  public boolean goToZero() {
+    setPos(ElevatorConstants.kStowElevator);
+    return isFinished();
+  }
+
+  @AfterHealthCheck
+  public boolean returnToZero() {
+    setPos(ElevatorConstants.kStowElevator);
+    return isFinished();
+  }
+
   public void setPos(double location) {
     if (desiredPosition != location) logger.info("Elevator moving to {}", location);
     desiredPosition = location;
diff --git a/src/main/java/frc/robot/subsystems/HandSubsystem.java b/src/main/java/frc/robot/subsystems/HandSubsystem.java
@@ -11,13 +11,28 @@
 import java.util.Set;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.healthcheck.AfterHealthCheck;
+import org.strykeforce.healthcheck.BeforeHealthCheck;
+import org.strykeforce.healthcheck.HealthCheck;
+import org.strykeforce.healthcheck.Position;
+import org.strykeforce.healthcheck.Timed;
 import org.strykeforce.telemetry.TelemetryService;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
 
 public class HandSubsystem extends MeasurableSubsystem {
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
+
+  @HealthCheck
+  @Position(
+      percentOutput = {-0.2, 0.2},
+      encoderChange = 300)
   private TalonSRX handLeftTalon;
+
+  @HealthCheck
+  @Timed(
+      percentOutput = {0.3, -0.3},
+      duration = 5)
   private TalonSRX rollerTalon;
   // private TalonSRX handRightTalon;
 
@@ -69,6 +84,18 @@ public HandSubsystem(Constants constants) {
     // handRightZeroStableCounts = 0;
   }
 
+  @BeforeHealthCheck
+  public boolean goToZero() {
+    setPos(HandConstants.kMaxFwd);
+    return isFinished();
+  }
+
+  @AfterHealthCheck
+  public boolean returnToZero() {
+    setPos(HandConstants.kMaxFwd);
+    return isFinished();
+  }
+
   public void runRollers(double percent) {
     if (percent != lastPercent) logger.info("Running rollers at {}", percent);
     rollerTalon.set(TalonSRXControlMode.PercentOutput, percent);
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -12,7 +12,10 @@
 import java.util.Set;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.healthcheck.AfterHealthCheck;
+import org.strykeforce.healthcheck.BeforeHealthCheck;
 import org.strykeforce.healthcheck.HealthCheck;
+import org.strykeforce.healthcheck.Position;
 import org.strykeforce.healthcheck.Timed;
 import org.strykeforce.telemetry.TelemetryService;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
@@ -21,13 +24,18 @@
 public class IntakeSubsystem extends MeasurableSubsystem {
   private IntakeState currIntakeState = IntakeState.RETRACTED;
 
-  @HealthCheck
+  @HealthCheck(order = 1)
   @Timed(
       percentOutput = {0.5, -0.5},
       duration = 5)
   private TalonFX intakeFalcon;
 
+  @HealthCheck(order = 2)
+  @Position(
+      percentOutput = {-0.1, 0.5},
+      encoderChange = 1000)
   private TalonSRX extendTalon;
+
   private double intakeSetPointTicks;
   private boolean isIntakeExtended = false;
   private Timer ejectTimer = new Timer();
@@ -55,6 +63,18 @@ public IntakeSubsystem(Constants constants) {
     zeroIntake();
   }
 
+  @BeforeHealthCheck
+  public boolean goToZero() {
+    retractIntake();
+    return isFinished();
+  }
+
+  @AfterHealthCheck
+  public boolean returnToZero() {
+    retractIntake();
+    return isFinished();
+  }
+
   public void intakeOpenLoop(double percentOutput) {
     intakeFalcon.set(ControlMode.PercentOutput, percentOutput);
   }
diff --git a/src/main/java/frc/robot/subsystems/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/ShoulderSubsystem.java
@@ -4,16 +4,31 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import frc.robot.Constants;
+import frc.robot.Constants.ShoulderConstants;
 import java.util.Set;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.healthcheck.AfterHealthCheck;
+import org.strykeforce.healthcheck.BeforeHealthCheck;
+import org.strykeforce.healthcheck.Follow;
+import org.strykeforce.healthcheck.HealthCheck;
+import org.strykeforce.healthcheck.Position;
 import org.strykeforce.telemetry.TelemetryService;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
 
 public class ShoulderSubsystem extends MeasurableSubsystem implements ArmComponent {
+
+  @HealthCheck
+  @Position(
+      percentOutput = {0.2, -0.2},
+      encoderChange = 1000)
   private TalonSRX leftMainShoulderTalon;
+
+  @HealthCheck
+  @Follow(leader = ShoulderConstants.kShoulderId)
   private TalonSRX rightFollowerShoulderTalon;
+
   private double desiredPosition;
   private Constants constants;
 
@@ -42,6 +57,18 @@ public ShoulderSubsystem(Constants constants) {
     desiredPosition = getPos();
   }
 
+  @BeforeHealthCheck
+  public boolean goToZero() {
+    setPos(0.0);
+    return isFinished();
+  }
+
+  @AfterHealthCheck
+  public boolean returnToZero() {
+    setPos(0.0);
+    return isFinished();
+  }
+
   public void setPos(double location) {
     if (location != desiredPosition) logger.info("Moving Shoulder to: {}", location);
     desiredPosition = location;
