elastic

diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
index 253bc855c9ea5028e715575c9b2a289185e5501b..3d3e702fa3af29b5fbdef6ce3470eb0369f0aadb 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
+++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
@@ -2,6 +2,7 @@ package frc.robot.commands.gpm;
 
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.hood.Hood;
 import frc.robot.subsystems.spindexer.Spindexer;
@@ -51,6 +52,7 @@ public class RunSpindexer extends Command {
                 reversing = false;
             }
         }
+        SmartDashboard.putBoolean("Spindexer Jamming", reversing);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index e9b2e6fd1e5023b9e4aa48e71661578a396c9715..f6817c71b4783d2079a911d0ce43705ed34c4bae 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -206,11 +206,11 @@ public class Turret extends SubsystemBase implements TurretIO{
                        if(calibrationDebouncer.calculate(calibrated)){
                                stopCalibrating();
                        }
-               } else{
+               } else {
                        // Sets motor control with feedforward
-                       // motor.setControl(mmVoltageRequest
-                       // .withPosition(motorGoalRotations)
-                       // .withFeedForward(robotTurnCompensation));
+                       motor.setControl(mmVoltageRequest
+                       .withPosition(motorGoalRotations)
+                       .withFeedForward(robotTurnCompensation));
                }
 
         Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());

diff --git a/src/main/java/frc/robot/util/HubActive.java b/src/main/java/frc/robot/util/HubActive.java
index 584cb9c8d87478ddd1945526af9e1fb54bbab096..1d6296a58ced8767cef31e8272d6dd61d42de2b5 100644 (file)
--- a/src/main/java/frc/robot/util/HubActive.java
+++ b/src/main/java/frc/robot/util/HubActive.java
@@ -4,6 +4,7 @@ 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 {
 
@@ -45,7 +46,7 @@ public class HubActive {
       case Red -> !redInactiveFirst;
       case Blue -> redInactiveFirst;
     };
-
+    SmartDashboard.putNumber("Time till active", timeToActive().orElse(timeToUnactive().orElse(0.0)));
     if (matchTime > 130) {
       // Transition shift, hub is active.
       return true;
@@ -147,6 +148,90 @@ public class HubActive {
       return Optional.of(0.0);
     }
   }
+
+  static public Optional<Double> timeToUnactive() {
+
+    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);
+    } 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 boolean wonAuto() {
     String gameData = DriverStation.getGameSpecificMessage();
     // If we have no game data, we cannot compute, assume hub is active, as its
@@ -174,5 +259,4 @@ public class HubActive {
     return x;
 
   }
-
 }