Miscellaneous logic changes

src/main/java/frc/robot/RobotStates.java

@@ -9,6 +9,7 @@
 import frc.robot.operator.Operator;
 import frc.robot.pilot.Pilot;
 import frc.robot.swerve.Swerve;
+import frc.robot.vision.Vision;
 import frc.spectrumLib.Telemetry;
 import lombok.Getter;
 
@@ -21,20 +22,22 @@ public final class RobotStates {
     private static final Pilot pilot = Robot.getPilot();
     private static final Operator operator = Robot.getOperator();
     private static final Swerve swerve = Robot.getSwerve();
+    private static final Vision vision = Robot.getVision();
 
     @Getter public static State appliedState = State.IDLE;
+    @Getter public static double thresholdSpeed = 1.0;
 
     public static final Trigger robotInNeutralZone = swerve.inNeutralZone();
     public static final Trigger robotInEnemyZone = swerve.inEnemyAllianceZone();
     public static final Trigger robotInFeedZone = robotInEnemyZone.or(robotInNeutralZone);
     public static final Trigger robotInScoreZone = robotInFeedZone.not();
 
     public static final Trigger forceScore = operator.AButton;
-    public static final Trigger hopperFull = new Trigger(() -> true);
+    public static final Trigger hopperFull = new Trigger(operator.BButton); //TODO: indexer current increases? velocity decreases? 
 
     //placeholders
-    private static final Trigger movementStable = new Trigger(() -> true);
-    private static final Trigger visionStable = new Trigger(() -> true);
+    private static final Trigger movementStable = new Trigger(swerve.overSpeedTrigger(thresholdSpeed).not());
+    private static final Trigger visionStable = new Trigger(() -> vision.hasAccuratePose()); //TODO: change with potentially better logic
 
     public static final Trigger robotReadyScore = (robotInScoreZone).and(movementStable).and(visionStable);
 
@@ -91,9 +94,10 @@ private static void toggleToState(Trigger button, State toggledState) {
                         () -> {
                             State next = (appliedState == toggledState) ? State.IDLE : toggledState;
                             appliedState = next;
-                            applyState(next);
-                        })
-        );
+                            SmartDashboard.putString("APPLIED STATE", next.toString());
+                            coordinator.applyRobotState(next);
+                        },
+                        swerve));
     }
 
     private static void pressToState(Trigger button, State pressedState) {

src/main/java/frc/robot/swerve/Swerve.java

@@ -281,33 +281,14 @@ public Trigger inFieldLeft() {
         return new Trigger(() -> getRobotPose().getY() >= halfWidth);
     }
 
-    private static final double DEFAULT_OVERSPEED_METERS_PER_SECOND = 10.0;
-
-    /**
-     * Check if the robot's linear velocity (vx, vy) magnitude exceeds the given threshold.
-     *
-     * @param thresholdMetersPerSecond speed threshold in m/s
-     * @return true if current linear speed > threshold
-     */
-    public boolean isGoingTooFast(double thresholdMetersPerSecond) {
+    public boolean isGoingTooFast(double thresholdSpeed) {
         ChassisSpeeds speeds = getCurrentRobotChassisSpeeds();
         double linearSpeed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
-        return linearSpeed > thresholdMetersPerSecond;
-    }
-
-    /** Check against the default threshold (10 m/s). */
-    public boolean isGoingTooFast() {
-        return isGoingTooFast(DEFAULT_OVERSPEED_METERS_PER_SECOND);
-    }
-
-    /** Trigger that becomes active when the robot exceeds the default overspeed threshold. */
-    public Trigger overSpeedTrigger() {
-        return overSpeedTrigger(DEFAULT_OVERSPEED_METERS_PER_SECOND);
+        return linearSpeed > thresholdSpeed;
     }
 
-    /** Trigger that becomes active when the robot exceeds the provided threshold. */
-    public Trigger overSpeedTrigger(double thresholdMetersPerSecond) {
-        return new Trigger(() -> isGoingTooFast(thresholdMetersPerSecond));
+    public Trigger overSpeedTrigger(double thresholdSpeed) {
+        return new Trigger(() -> isGoingTooFast(thresholdSpeed));
     }
     /**
      * This method is used to check if the robot is in the X zone of the field flips the values if

src/main/java/frc/robot/swerve/SwerveStates.java

@@ -50,7 +50,10 @@ protected static void setupDefaultCommand() {
 
     //define Triggers here
     private static final Trigger inSnakeDrive = new Trigger(() -> RobotStates.getAppliedState() == State.SNAKE_INTAKE);
-    private static final Trigger inScoreOrFeed = new Trigger(() -> RobotStates.getAppliedState() == State.TURRET_WITHOUT_TRACK_WITH_LAUNCH || RobotStates.getAppliedState() == State.TURRET_FEED_WITH_LAUNCH || RobotStates.getAppliedState() == State.TURRET_TRACK_WITH_LAUNCH);
+    private static final Trigger inScoreOrFeed = new Trigger(() -> 
+        RobotStates.getAppliedState() == State.TURRET_WITHOUT_TRACK_WITH_LAUNCH 
+        || RobotStates.getAppliedState() == State.TURRET_FEED_WITH_LAUNCH 
+        || RobotStates.getAppliedState() == State.TURRET_TRACK_WITH_LAUNCH);
 
     protected static void setStates() {
         // Force back to manual steering when we steer