Add pre-match checks

team422 · May 30, 2024 · 13edcd4 · 13edcd4
1 parent db23cd1
commit 13edcd4
Show file tree

Hide file tree

Showing 6 changed files with 56 additions and 17 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -97,15 +97,15 @@ public static final class ShooterConstants {
   public static final class IntakeConstants {
     public static final double kPivotGearRatio = 36.0 / 16;
 
-    public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(124);
+    public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(125);
     public static final Rotation2d kMinAngle = Rotation2d.fromDegrees(5);
-    public static final Rotation2d kHomeAngle = Rotation2d.fromDegrees(120);
+    public static final Rotation2d kHomeAngle = Rotation2d.fromDegrees(124);
     public static final Rotation2d kDeployedAngle = Rotation2d.fromDegrees(15);
 
     // don't ask me why it's negative, just a hardware thing
     public static final double kDeployRollerVoltage = -6.0;
 
-    public static final Rotation2d kPivotOffset = Rotation2d.fromDegrees(123);
+    public static final Rotation2d kPivotOffset = Rotation2d.fromDegrees(124);
 
     public static final LoggedTunableNumber kPivotP =
         new LoggedTunableNumber("Intake Pivot P", 3.0);

diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
@@ -16,6 +16,7 @@
 import frc.robot.subsystems.led.Led;
 import frc.robot.subsystems.led.Led.LedState;
 import frc.robot.subsystems.shooter.Shooter;
+import java.util.HashMap;
 import org.littletonrobotics.junction.Logger;
 
 public class RobotState {
@@ -48,6 +49,8 @@ public class RobotState {
           new Translation3d(-0.269, -0.01, 0.2428),
           new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(201), 0.0));
 
+  private final HashMap<String, Boolean> kPrematchCheckValues;
+
   private RobotState(Drive drive, Shooter shooter, Indexer indexer, Intake intake, Led led) {
     // subsystems
     m_drive = drive;
@@ -91,6 +94,13 @@ private RobotState(Drive drive, Shooter shooter, Indexer indexer, Intake intake,
 
     // post to dashboard
     SmartDashboard.putData("Mech2d", m_mechanism);
+
+    kPrematchCheckValues = new HashMap<>();
+    kPrematchCheckValues.put("ShooterPivot", false);
+    kPrematchCheckValues.put("FlywheelsStill", false);
+    kPrematchCheckValues.put("IntakePivot", false);
+    kPrematchCheckValues.put("RollersStill", false);
+    kPrematchCheckValues.put("IndexerMotorsStill", false);
   }
 
   public static RobotState startInstance(
@@ -118,6 +128,17 @@ public void updateRobotState() {
     if (kComponentsEnabled) {
       updateComponents();
     }
+
+    boolean prematchReady = true;
+    for (String key : kPrematchCheckValues.keySet()) {
+      Logger.recordOutput("PreMatchCheck/" + key, kPrematchCheckValues.get(key));
+      prematchReady &= kPrematchCheckValues.get(key);
+    }
+    if (edu.wpi.first.wpilibj.RobotState.isDisabled() && prematchReady) {
+      m_led.setState(LedState.DISABLED_READY);
+    } else {
+      m_led.setState(LedState.DISABLED_NOT_READY);
+    }
   }
 
   public void updateMechanism() {
@@ -157,6 +178,13 @@ public void onEnableAutonomous() {
   }
 
   public void onDisable() {
-    m_led.setState(LedState.DISABLED);
+    m_led.setState(LedState.DISABLED_NOT_READY);
+  }
+
+  public void setPrematchCheckValue(String key, boolean value) {
+    if (!kPrematchCheckValues.containsKey(key)) {
+      throw new IllegalArgumentException("Invalid prematch check key: " + key);
+    }
+    kPrematchCheckValues.put(key, value);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotState;
 import org.littletonrobotics.junction.Logger;
 
 public class Indexer extends SubsystemBase {
@@ -21,9 +22,11 @@ public void periodic() {
     Logger.processInputs("Indexer", m_inputs);
 
     // ready for match
-    Logger.recordOutput(
-        "ReadyForMatch/Indexer",
-        m_inputs.curFeederVelocity < 0.1 && m_inputs.curKickerVelocity < 0.1);
+    RobotState.getInstance()
+        .setPrematchCheckValue(
+            "IndexerMotorsStill",
+            Math.abs(m_inputs.curFeederVelocity) < 0.1
+                && Math.abs(m_inputs.curKickerVelocity) < 0.1);
   }
 
   public void setKickerVoltage(double voltage) {

diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.lib.utils.LoggedTunableNumber;
 import frc.robot.Constants.IntakeConstants;
+import frc.robot.RobotState;
 import frc.robot.subsystems.intake.pivot.IntakePivotIO;
 import frc.robot.subsystems.intake.pivot.IntakePivotInputsAutoLogged;
 import frc.robot.subsystems.intake.roller.RollerIO;
@@ -62,8 +63,9 @@ public void periodic() {
         "Intake/Pivot/CurrentAngle", Units.radiansToDegrees(m_pivotInputs.curAngle));
 
     // ready for match
-    Logger.recordOutput(
-        "ReadyForMatch/Intake", m_pivotController.atGoal() && m_rollerInputs.curVelocity < 0.1);
+    RobotState.getInstance().setPrematchCheckValue("IntakePivot", m_pivotController.atGoal());
+    RobotState.getInstance()
+        .setPrematchCheckValue("RollersStill", Math.abs(m_rollerInputs.curVelocity) < 0.1);
   }
 
   public void setRollerVoltage(double voltage) {

diff --git a/src/main/java/frc/robot/subsystems/led/Led.java b/src/main/java/frc/robot/subsystems/led/Led.java
@@ -15,7 +15,8 @@ public static enum LedState {
     OFF,
     AUTONOMOUS,
     TELEOP,
-    DISABLED
+    DISABLED_NOT_READY,
+    DISABLED_READY
   }
 
   public Led(int port, int length) {
@@ -50,11 +51,14 @@ public void setState(LedState state) {
         setSolidColor(Color.kBlue);
         break;
       case TELEOP:
-        setSolidColor(Color.kGreen);
+        setSolidColor(Color.kMagenta);
         break;
-      case DISABLED:
+      case DISABLED_NOT_READY:
         setSolidColor(Color.kRed);
         break;
+      case DISABLED_READY:
+        setSolidColor(Color.kGreen);
+        break;
     }
     m_state = state;
   }

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.lib.utils.LoggedTunableNumber;
 import frc.robot.Constants.ShooterConstants;
+import frc.robot.RobotState;
 import frc.robot.subsystems.shooter.flywheel.FlywheelIO;
 import frc.robot.subsystems.shooter.flywheel.FlywheelInputsAutoLogged;
 import frc.robot.subsystems.shooter.pivot.ShooterPivotIO;
@@ -177,11 +178,12 @@ public void periodic() {
         "Shooter/Flywheel/RightFlywheelDesiredVelocity", m_rightFlywheelController.getSetpoint());
 
     // ready for match
-    Logger.recordOutput(
-        "ReadyForMatch/Shooter",
-        m_pivotController.atGoal()
-            && m_flywheelInputs.linearVelocity[0] < 0.1
-            && m_flywheelInputs.linearVelocity[1] < 0.1);
+    RobotState.getInstance().setPrematchCheckValue("ShooterPivot", m_pivotController.atGoal());
+    RobotState.getInstance()
+        .setPrematchCheckValue(
+            "FlywheelsStill",
+            Math.abs(m_flywheelInputs.linearVelocity[0]) < 0.1
+                && Math.abs(m_flywheelInputs.linearVelocity[1]) < 0.1);
   }
 
   public void setPivotAngle(Rotation2d angle) {