may be braking drivetrain, idk, wouldn't make sense tho. Hub timer

diff --git a/src/main/java/frc/robot/util/HubActive.java b/src/main/java/frc/robot/util/HubActive.java
index 584cb9c8d87478ddd1945526af9e1fb54bbab096..d0c5b3ee264965fd2a3b03adc06d34815da77354 100644 (file)
--- a/src/main/java/frc/robot/util/HubActive.java
+++ b/src/main/java/frc/robot/util/HubActive.java
@@ -1,12 +1,17 @@
 package frc.robot.util;
 
+
 import java.util.Optional;
 
+
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 
 public class HubActive {
 
+
   static public boolean isHubActive() {
     Optional<Alliance> alliance = DriverStation.getAlliance();
     // If we have no alliance, we cannot be enabled, therefore no hub.
@@ -22,6 +27,7 @@ public class HubActive {
       return false;
     }
 
+
     // We're teleop enabled, compute.
     double matchTime = DriverStation.getMatchTime();
     String gameData = DriverStation.getGameSpecificMessage();
@@ -40,12 +46,13 @@ public class HubActive {
       }
     }
 
+
     // Shift was is active for blue if red won auto, or red if blue won auto.
     boolean shift1Active = switch (alliance.get()) {
       case Red -> !redInactiveFirst;
       case Blue -> redInactiveFirst;
     };
-
+    SmartDashboard.putNumber("Time till active", timeToActive().orElse(timeToInactive().orElse(0.0)));
     if (matchTime > 130) {
       // Transition shift, hub is active.
       return true;
@@ -67,8 +74,10 @@ public class HubActive {
     }
   }
 
+
   static public Optional<Double> timeToActive() {
 
+
     Optional<Alliance> alliance = DriverStation.getAlliance();
     // If we have no alliance, we cannot be enabled, therefore no hub.
     if (alliance.isEmpty()) {
@@ -83,6 +92,7 @@ public class HubActive {
       return Optional.empty();
     }
 
+
     // We're teleop enabled, compute.
     double matchTime = DriverStation.getMatchTime();
     String gameData = DriverStation.getGameSpecificMessage();
@@ -102,12 +112,108 @@ public class HubActive {
     }
 
 
+
+
     // Shift was is active for blue if red won auto, or red if blue won auto.
     boolean shift1Active = switch (alliance.get()) {
       case Red -> !redInactiveFirst;
       case Blue -> redInactiveFirst;
     };
 
+
+    if (matchTime > 130) {
+      // Transition shift, hub is active.
+      return Optional.of(0.0);
+    } else if (matchTime > 105) {
+      // Shift 1
+      if (shift1Active) {
+        return Optional.of(matchTime - 105.0);
+      } else {
+        return Optional.empty();
+      }
+    } else if (matchTime > 80) {
+      // Shift 2
+
+
+      if (!shift1Active) {
+        return Optional.of(matchTime - 80.0);
+      } else {
+        return Optional.empty();
+      }
+    } else if (matchTime > 55) {
+      // Shift 3
+
+
+      if (shift1Active) {
+        return Optional.of(matchTime - 55.0);
+      } else {
+        return Optional.empty();
+      }
+    } else if (matchTime > 30) {
+      // Shift 4
+
+
+      if (!shift1Active) {
+        return Optional.of(matchTime - 30.0);
+      } else {
+        return Optional.empty();
+      }
+    } else {
+      // End game, hub always active.
+      return Optional.of(0.0);
+    }
+  }
+
+
+  static public Optional<Double> timeToInactive() {
+
+
+    Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue);
+    if (alliance == Alliance.Blue) {
+      alliance = Alliance.Red;
+    } else {
+      alliance = Alliance.Blue;
+    }
+
+
+    // Hub is always enabled in autonomous.
+    if (DriverStation.isAutonomousEnabled()) {
+      return Optional.of(0.0);
+    }
+    // At this point, if we're not teleop enabled, there is no hub.
+    if (!DriverStation.isTeleopEnabled()) {
+      return Optional.empty();
+    }
+
+
+    // We're teleop enabled, compute.
+    double matchTime = DriverStation.getMatchTime();
+    String gameData = DriverStation.getGameSpecificMessage();
+    // If we have no game data, we cannot compute, assume hub is active, as its
+    // likely early in teleop.
+    if (gameData.isEmpty()) {
+      return Optional.of(0.0);
+    }
+    boolean redInactiveFirst = false;
+    switch (gameData.charAt(0)) {
+      case 'R' -> redInactiveFirst = true;
+      case 'B' -> redInactiveFirst = false;
+      default -> {
+        // If we have invalid game data, assume hub is active.
+        return Optional.of(0.0);
+      }
+    }
+
+
+
+
+    // Shift was is active for blue if red won auto, or red if blue won auto.
+    boolean shift1Active = switch (alliance) {
+      case Red -> !redInactiveFirst;
+      case Blue -> redInactiveFirst;
+    };
+
+
     if (matchTime > 130) {
       // Transition shift, hub is active.
       return Optional.of(0.0);
@@ -121,6 +227,7 @@ public class HubActive {
     } else if (matchTime > 80) {
       // Shift 2
 
+
       if (!shift1Active) {
         return Optional.of(matchTime - 80.0);
       } else {
@@ -129,6 +236,7 @@ public class HubActive {
     } else if (matchTime > 55) {
       // Shift 3
 
+
       if (shift1Active) {
         return Optional.of(matchTime - 55.0);
       } else {
@@ -137,6 +245,7 @@ public class HubActive {
     } else if (matchTime > 30) {
       // Shift 4
 
+
       if (!shift1Active) {
         return Optional.of(matchTime - 30.0);
       } else {
@@ -147,6 +256,8 @@ public class HubActive {
       return Optional.of(0.0);
     }
   }
+
+
   static public boolean wonAuto() {
     String gameData = DriverStation.getGameSpecificMessage();
     // If we have no game data, we cannot compute, assume hub is active, as its
@@ -164,6 +275,7 @@ public class HubActive {
       }
     }
 
+
     var alliance = DriverStation.getAlliance().get();
     boolean x;
     if (alliance == Alliance.Red) {
@@ -173,6 +285,6 @@ public class HubActive {
     }
     return x;
 
-  }
 
-}
+  }
+}
\ No newline at end of file