From a7cca7464f75925d00d75f341cc1018be01effa4 Mon Sep 17 00:00:00 2001 From: Spectrum Date: Wed, 11 Feb 2026 19:14:56 -0600 Subject: [PATCH 1/2] logic cleanup + additional constraints --- src/main/java/frc/robot/RobotStates.java | 12 +++++---- src/main/java/frc/robot/swerve/Swerve.java | 27 +++---------------- .../java/frc/robot/swerve/SwerveStates.java | 5 +++- 3 files changed, 15 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 4c958ce..4d731d0 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -23,6 +23,7 @@ public final class RobotStates { private static final Swerve swerve = Robot.getSwerve(); @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(); @@ -30,10 +31,10 @@ public final class RobotStates { 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: replace with actual hopper full condition //placeholders - private static final Trigger movementStable = new Trigger(() -> true); + private static final Trigger movementStable = new Trigger(swerve.overSpeedTrigger(thresholdSpeed).not()); private static final Trigger visionStable = new Trigger(() -> true); public static final Trigger robotReadyScore = (robotInScoreZone).and(movementStable).and(visionStable); @@ -91,9 +92,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) { diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index b81fbb4..5044bf1 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/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 diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index 7e9cc88..023f757 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/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 From 26738ea19404752a4530aa8ac9e2b57706e3a7fb Mon Sep 17 00:00:00 2001 From: Spectrum Date: Wed, 11 Feb 2026 19:38:19 -0600 Subject: [PATCH 2/2] further trigger changes --- src/main/java/frc/robot/RobotStates.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 4d731d0..392ae08 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/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,6 +22,7 @@ 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; @@ -31,11 +33,11 @@ public final class RobotStates { public static final Trigger robotInScoreZone = robotInFeedZone.not(); public static final Trigger forceScore = operator.AButton; - public static final Trigger hopperFull = new Trigger(operator.BButton); //TODO: replace with actual hopper full condition + public static final Trigger hopperFull = new Trigger(operator.BButton); //TODO: indexer current increases? velocity decreases? //placeholders private static final Trigger movementStable = new Trigger(swerve.overSpeedTrigger(thresholdSpeed).not()); - private static final Trigger visionStable = new Trigger(() -> true); + 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);