w0n autos

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 65ad9f231d96be6d56b294ed42cf608397d76c70..5e751e2e5409a74c54fe4d3c09b631d96f04c49d 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
+import frc.robot.util.HubActive;
 
 public class Shooter extends SubsystemBase implements ShooterIO {
     
@@ -30,6 +31,7 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
     private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
 
+
     double powerModifier = 1.05; // TESTED
 
     public Shooter(){
@@ -78,6 +80,8 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         
         Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER);
         Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed);
+
+        SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOON" : "lost");
     }
 
     /**

diff --git a/src/main/java/frc/robot/util/HubActive.java b/src/main/java/frc/robot/util/HubActive.java
index dd5b76255f936d63aca9e89ebcfd1531491a9ccd..584cb9c8d87478ddd1945526af9e1fb54bbab096 100644 (file)
--- a/src/main/java/frc/robot/util/HubActive.java
+++ b/src/main/java/frc/robot/util/HubActive.java
@@ -101,6 +101,7 @@ 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;
@@ -146,4 +147,32 @@ 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
+    // likely early in teleop.
+    if (gameData.isEmpty()) {
+      return false;
+    }
+    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 false;
+      }
+    }
+
+    var alliance = DriverStation.getAlliance().get();
+    boolean x;
+    if (alliance == Alliance.Red) {
+      x = redInactiveFirst;
+    } else {
+      x = !redInactiveFirst;
+    }
+    return x;
+
+  }
+
 }