54 add shift detection

.vscode/settings.json

@@ -75,5 +75,5 @@
     "Coppercore",
     "Tunables"
   ],
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
 }

ci-scripts/check_constants.py

@@ -27,6 +27,7 @@
     "ShotMaps.json": "frc.robot.constants.ShotMaps",
     "RedFieldLocations.json": "frc.robot.constants.FieldLocationInstance",
     "BlueFieldLocations.json": "frc.robot.constants.FieldLocationInstance",
+    "StrategyConstants.json": "frc.robot.constants.StrategyConstants",
 }
 
 SKIP_FILES = {"config.json", "controllers-xbox.json"}

src/main/deploy/constants/comp/StrategyConstants.json

@@ -0,0 +1,10 @@
+{
+  "shiftStartGracePeriod": {
+    "value": 2.0,
+    "unit": "Second"
+  },
+  "shiftEndGracePeriod": {
+    "value": 2.0,
+    "unit": "Second"
+  }
+}

src/main/deploy/constants/comp/controllers-xbox.json

@@ -63,6 +63,28 @@
           }
         }
       ]
+    },
+    {
+      "port": 1,
+      "type": "xbox",
+      "controlElements": [
+        {
+          "command": "redWonAuto",
+          "commandType": "button",
+          "hapticControlElement": {
+            "elementType": "button",
+            "id": "b"
+          }
+        },
+        {
+          "command": "blueWonAuto",
+          "commandType": "button",
+          "hapticControlElement": {
+            "elementType": "button",
+            "id": "x"
+          }
+        }
+      ]
     }
   ]
 }

src/main/java/frc/robot/ControllerSetup.java

@@ -86,4 +86,16 @@ public static void initIntakeBindings(IntakeSubsystem intakeSubsystem) {
                 () -> intakeSubsystem.runRollers(JsonConstants.intakeConstants.intakeRollerSpeed)))
         .onFalse(new InstantCommand(intakeSubsystem::stopRollers));
   }
+
+  public static void initMatchStateBindings(CoordinationLayer coordinationLayer) {
+    var controllers = getControllers();
+    controllers
+        .getButton("redWonAuto")
+        .getTrigger()
+        .onTrue(new InstantCommand(coordinationLayer::overrideMatchStateRed));
+    controllers
+        .getButton("blueWonAuto")
+        .getTrigger()
+        .onTrue(new InstantCommand(coordinationLayer::overrideMatchStateBlue));
+  }
 }

src/main/java/frc/robot/CoordinationLayer.java

@@ -14,12 +14,14 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.DependencyOrderedExecutor.ActionKey;
 import frc.robot.ShotCalculations.MapBasedShotInfo;
 import frc.robot.ShotCalculations.ShotInfo;
 import frc.robot.ShotCalculations.ShotTarget;
 import frc.robot.constants.FieldConstants;
 import frc.robot.constants.JsonConstants;
+import frc.robot.coordination.MatchState;
 import frc.robot.subsystems.HomingSwitch;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveCoordinator;
@@ -30,6 +32,7 @@
 import frc.robot.subsystems.shooter.ShooterSubsystem;
 import frc.robot.subsystems.turret.TurretSubsystem;
 import java.util.Optional;
+import org.littletonrobotics.junction.Logger;
 
 /**
  * The coordination layer is responsible for updating subsystem dependencies, running the shot
@@ -61,14 +64,28 @@ public class CoordinationLayer {
       new ActionKey("CoordinationLayer::runShotCalculator");
   public static final ActionKey RUN_DEMO_MODES =
       new ActionKey("CoordinationLayer::runSubsystemDemoModes");
-
+  public static final ActionKey UPDATE_MATCH_STATE =
+      new ActionKey("CoordinationLayer::updateMatchState");
+  public static final ActionKey OVERRIDE_MATCH_STATE_RED =
+      new ActionKey("CoordinationLayer::overrideMatchStateRed");
+  public static final ActionKey OVERRIDE_MATCH_STATE_BLUE =
+      new ActionKey("CoordinationLayer::overrideMatchStateBlue");
   // State variables (these will be updated by various methods and then their values will be passed
   // to subsystems during the execution of a cycle)
+  private final MatchState matchState = new MatchState();
+  // Manual override requests set by controller bindings. These are cleared once consumed in
+  // updateMatchState and forwarded to MatchState.enabledPeriodic as booleans.
+  private volatile boolean manualRedOverrideRequest = false;
+  private volatile boolean manualBlueOverrideRequest = false;
+
   public CoordinationLayer(DependencyOrderedExecutor dependencyOrderedExecutor) {
     this.dependencyOrderedExecutor = dependencyOrderedExecutor;
 
+    dependencyOrderedExecutor.registerAction(UPDATE_MATCH_STATE, this::updateMatchState);
     dependencyOrderedExecutor.registerAction(RUN_SHOT_CALCULATOR, this::runShotCalculator);
     dependencyOrderedExecutor.registerAction(RUN_DEMO_MODES, this::runSubsystemDemoModes);
+
+    dependencyOrderedExecutor.addDependencies(RUN_SHOT_CALCULATOR, UPDATE_MATCH_STATE);
   }
 
   /**
@@ -211,6 +228,9 @@ private void updateIntakeDependencies(IntakeSubsystem intake) {
   }
 
   // Coordination and processing
+  /** Coordinates subsystem actions based on the desired action and subsystem inputs */
+  public void coordinateSubsystemActions() {}
+
   /**
    * Runs the shot calculator and logs the resulting trajectories for debugging. Eventually, this
    * method should also command the subsystems to take their actinos.
@@ -331,6 +351,33 @@ private void runSubsystemDemoModes() {
     }
   }
 
+  /** Update the MatchState each periodic loop */
+  private void updateMatchState() {
+    if (DriverStation.isEnabled()) {
+      matchState.enabledPeriodic(manualRedOverrideRequest, manualBlueOverrideRequest);
+    }
+
+    double timeLeft = matchState.getTimeLeftInCurrentShift();
+
+    boolean hasFiveSecondsLeft = timeLeft >= 5.0;
+    Logger.recordOutput("MatchState/has5sLeft", hasFiveSecondsLeft);
+  }
+
+  /**
+   * Called by a controller binding or DOE action to request that the match state be overridden to
+   * red for the next enabledPeriodic call. This sets a one-shot request which is consumed by
+   * updateMatchState.
+   */
+  public void overrideMatchStateRed() {
+    manualRedOverrideRequest = true;
+    manualBlueOverrideRequest = false;
+  }
+
+  public void overrideMatchStateBlue() {
+    manualBlueOverrideRequest = true;
+    manualRedOverrideRequest = false;
+  }
+
   /**
    * Given an "ideal" shot, command the scoring subsystems to target it
    *

src/main/java/frc/robot/DependencyOrderedExecutor.java

@@ -66,7 +66,7 @@ public DependencyOrderedExecutor(Time period) {
   }
 
   /**
-   * Register an action, setting the Runnable associated with an ActionKey
+   * Register an action to run in each periodic, setting the Runnable associated with an ActionKey
    *
    * @param key The key of the action to register
    * @param action The Runnable to run in order to execute this action

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

@@ -191,6 +191,7 @@ private void configureButtonBindings() {
           drive.ifPresent(
               (drive) -> {
                 ControllerSetup.initDriveBindings(driveCoordinator, drive);
+                ControllerSetup.initMatchStateBindings(coordinationLayer);
               });
         });
 

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

@@ -77,6 +77,7 @@ public static void loadConstants() {
         jsonHandler.getObject(new FieldLocationInstance(), "RedFieldLocations.json");
     blueFieldLocations =
         jsonHandler.getObject(new FieldLocationInstance(), "BlueFieldLocations.json");
+    strategyConstants = jsonHandler.getObject(new StrategyConstants(), "StrategyConstants.json");
 
     if (featureFlags.useTuningServer) {
       // do not crash Robot if routes could not be added for any reason
@@ -128,6 +129,7 @@ public static void loadConstants() {
   public static ShotMaps shotMaps;
   public static FieldLocationInstance redFieldLocations;
   public static FieldLocationInstance blueFieldLocations;
+  public static StrategyConstants strategyConstants;
 
   public static Controllers controllers;
 }

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

@@ -0,0 +1,54 @@
+package frc.robot.constants;
+
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.units.measure.Time;
+
+public class StrategyConstants {
+  // Due to the presence of these constants, it is necessary for the driver(s) to play an active
+  // role in deciding when to score.
+
+  /**
+   * How much grace period to "add" to the start of each shooting shift.
+   *
+   * <p>For example, a value of 5 seconds means that the robot will enable shooting features 5
+   * seconds before it estimates that an active shift begins
+   *
+   * <p>This constant is necessary because getMatchTime does not return an exact match result, and
+   * so the match time is simply an estimate.
+   */
+  public final Time shiftStartGracePeriod = Seconds.of(2.0);
+
+  /**
+   * How much grace period to "add" to the end of each shooting shift.
+   *
+   * <p>For example, a value of 5 seconds means that the robot will only disable shooting features 5
+   * seconds after it estimates that an active shift has ended
+   *
+   * <p>This constant is necessary because getMatchTime does not return an exact match result, and
+   * so the match time is simply an estimate.
+   */
+  public final Time shiftEndGracePeriod = Seconds.of(2.0);
+
+  // list of shift start times in seconds from the start of the match
+  // 2:20
+  public static final Double transitionStart = 2 * 60 + 20.0; // 140
+
+  // 2:10
+  public static final Double shift1Start = 2 * 60 + 10.0; // 130
+
+  // 1:45
+  public static final Double shift2Start = 60 + 45.0; // 105
+
+  // 1:20
+  public static final Double shift3Start = 60 + 20.0; // 80
+
+  // 0:55
+  public static final Double shift4Start = 55.0;
+
+  // 0:30
+  public static final Double endgameStart = 30.0;
+
+  // 0:00
+  public static final Double matchEnd = 0.0;
+}