54 add shift detection (#58)

* (#54) Add basic shift detection prototype

* Match shift tracking and added to CoordinationLayer

* change some of match state logic and added strategy constants for shift timing

* remove json save

* add constants to json

* Add manual override functionality for match state in CoordinationLayer and MatchState

* Add match state override button for blue

* change match time to start as negative infinity so that when it init's it doesnt think its auto

* almost finished. bug with red and blue override not changing the alliance that won auto

* fix auto winner bug

* auto winner changed to use Optional

* (#54) Fix various review comments and clean up code (will resolve comments later)

* (#54) Add strategy constants to CI checker and fix extra fields

* (#54) Add button bindings for red/blue winning auto

* (#54) Actually call bindings setup method and clean up getTimeLeftInCurrentShift by using a switch expression

* (#54) Allow for user correction if auto winner is incorrectly selected; Protect against null game data; log auto winner

* (#54) Remove old unused action keys

* (#54) Document temporary code, remove unused volatile designation, remove old comment

---------

Co-authored-by: aidnem <99768676+aidnem@users.noreply.github.com>
Co-authored-by: gback <gback@vt.edu>
Co-authored-by: Godmar Back <godmar@gmail.com>

Author: 3 people

.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"
 }

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,25 @@ 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");
 
   // 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 boolean manualRedOverrideRequest = false;
+  private 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 +225,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 +348,35 @@ private void runSubsystemDemoModes() {
     }
   }
 
+  /** Update the MatchState each periodic loop */
+  private void updateMatchState() {
+    if (DriverStation.isEnabled()) {
+      matchState.enabledPeriodic(manualRedOverrideRequest, manualBlueOverrideRequest);
+    }
+
+    // This is temporary code left here to make it easy to integrate the time left functionality
+    // with LEDs and superstructure coordination later.
+    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;
+}