From eb59683f7bfb4ad45f18c6a2c124609e76a1f903 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Fri, 30 Jan 2026 15:20:46 -0600 Subject: [PATCH 1/3] triggers and automatic state transitions --- simgui-window.json | 2 +- src/main/java/frc/robot/Coordinator.java | 2 +- src/main/java/frc/robot/RobotStates.java | 17 ++++++++++--- src/main/java/frc/robot/State.java | 1 + src/main/java/frc/robot/pilot/Pilot.java | 8 ++---- src/main/java/frc/robot/swerve/Swerve.java | 29 ++++++++++++++++++++++ 6 files changed, 47 insertions(+), 12 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index a261a03..ab79450 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -64,7 +64,7 @@ "###FMS": { "Collapsed": "0", "Pos": "268,27", - "Size": "202,214" + "Size": "210,214" }, "###Joysticks": { "Collapsed": "0", diff --git a/src/main/java/frc/robot/Coordinator.java b/src/main/java/frc/robot/Coordinator.java index 710aebe..5ebdde6 100644 --- a/src/main/java/frc/robot/Coordinator.java +++ b/src/main/java/frc/robot/Coordinator.java @@ -48,7 +48,7 @@ public void applyRobotState(State state) { } case TURRET_FEED_WITH_SPINUP -> { FuelIntakeStates.stop(); - IndexerStates.neutral(); + IndexerStates.indexMax(); IntakeExtensionStates.fullExtend(); LauncherStates.aimAtHub(); TurretHoodStates.aimAtHub(); diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 74451a1..9b95110 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -31,18 +31,27 @@ public class RobotStates { // Define triggers here public static final Trigger robotInNeutralZone = swerve.inNeutralZone(); public static final Trigger robotInEnemyZone = swerve.inEnemyAllianceZone(); - // public static final Trigger hopperFull = new Trigger(null); + public static final Trigger hopperFull = new Trigger(() -> true); + public static final Trigger robotReadyScore = new Trigger(() -> false); //movement stable + stable vision + public static final Trigger robotReadyFeed = new Trigger(hopperFull); //movement stable + vision stable + hopper full + public static final Trigger robotInScoreZone = robotInEnemyZone.not().and(robotInNeutralZone.not()); + public static final Trigger robotInFeedZone = robotInEnemyZone.or(robotInNeutralZone); // Setup any binding to set states public static void setupStates() { // Pilot Triggers pilot.AButton.onTrue(applyState(State.INTAKE_FUEL)); pilot.AButton.onFalse(applyState(State.IDLE)); - pilot.BButton.onTrue(applyState(State.TURRET_TRACK_WITH_SPINUP)); - pilot.BButton.onFalse(applyState(State.TURRET_TRACK_WITH_LAUNCH)); + pilot.BButton.onTrue(); + pilot.XButton.onTrue(applyState(State.TURRET_TRACK_WITH_SPINUP)); + pilot.YButton.onTrue(applyState(State.TURRET_FEED_WITH_SPINUP)); pilot.home_select.onTrue(clearState()); - robotInNeutralZone.or(robotInEnemyZone).whileTrue(applyState(State.TURRET_FEED_WITH_SPINUP)); + robotInScoreZone.and(robotReadyScore.not()).onTrue(applyState(State.TURRET_TRACK_WITH_SPINUP)); + robotInScoreZone.and(robotReadyScore).onTrue(applyState(State.TURRET_TRACK_WITH_LAUNCH)); + + robotInFeedZone.and(robotReadyFeed.not()).onTrue(applyState(State.TURRET_FEED_WITH_SPINUP)); + robotInFeedZone.and(robotReadyFeed).onTrue(applyState(State.TURRET_FEED_WITH_LAUNCH)); // Auton Triggers Auton.autonIntake.onTrue(applyState(State.INTAKE_FUEL)); diff --git a/src/main/java/frc/robot/State.java b/src/main/java/frc/robot/State.java index 148af26..138d2d3 100644 --- a/src/main/java/frc/robot/State.java +++ b/src/main/java/frc/robot/State.java @@ -9,6 +9,7 @@ public enum State { IDLE, INTAKE_FUEL, + SMART_INTAKE, TURRET_TRACK_WITH_SPINUP, TURRET_TRACK_WITH_LAUNCH, diff --git a/src/main/java/frc/robot/pilot/Pilot.java b/src/main/java/frc/robot/pilot/Pilot.java index 36630de..62d23e4 100644 --- a/src/main/java/frc/robot/pilot/Pilot.java +++ b/src/main/java/frc/robot/pilot/Pilot.java @@ -24,6 +24,8 @@ public class Pilot extends Gamepad { public final Trigger RT = rightTrigger.and(noFn, teleop, photon.not()); public final Trigger LB_LT = leftTrigger.and(fn, teleop, photon.not()); + public final Trigger AButton = A.and(teleop); + public final Trigger BButton = B.and(teleop); public final Trigger XButton = X.and(teleop); public final Trigger YButton = Y.and(teleop); @@ -34,12 +36,6 @@ public class Pilot extends Gamepad { // Vision Triggers public final Trigger tagsInView = new Trigger(() -> Robot.getVision().tagsInView()); - // vision Drive - // public final Trigger reefAim_A = A.and(teleop, tagsInView.not()); // turn off - public final Trigger AButton = A.and(teleop); // , tagsInView); // remove tags in view - public final Trigger BButton = B.and(teleop); - // public final Trigger cageAim_B = B.and(teleop); - // Drive Triggers public final Trigger upReorient = upDpad.and(fn, teleop); public final Trigger leftReorient = leftDpad.and(fn, teleop); diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 71afe1b..c5193fb 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -270,6 +270,35 @@ public Trigger inEnemyAllianceZone() { }); } + + 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) { + 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); + } + + /** Trigger that becomes active when the robot exceeds the provided threshold. */ + public Trigger overSpeedTrigger(double thresholdMetersPerSecond) { + return new Trigger(() -> isGoingTooFast(thresholdMetersPerSecond)); + } /** * This method is used to check if the robot is in the X zone of the field flips the values if * Red Alliance From 1261031e25a13e57f91caf50a443b9eeb41f1831 Mon Sep 17 00:00:00 2001 From: Spectrum Date: Sat, 7 Feb 2026 17:21:42 -0600 Subject: [PATCH 2/3] miscellaneous changes coordinator + state logic changes, tweak out while scoring for swerve, button mapping, and more --- simgui-ds.json | 21 +++++---- simgui-window.json | 9 +++- src/main/java/frc/rebuilt/ShotCalculator.java | 2 +- src/main/java/frc/robot/Coordinator.java | 43 +++++++++++++++---- src/main/java/frc/robot/RobotStates.java | 43 +++++++++++++------ src/main/java/frc/robot/State.java | 16 ++++--- .../java/frc/robot/launcher/Launcher.java | 8 ++-- .../frc/robot/launcher/LauncherStates.java | 18 ++++---- .../java/frc/robot/operator/Operator.java | 32 +++----------- src/main/java/frc/robot/swerve/Swerve.java | 15 ------- .../java/frc/robot/swerve/SwerveStates.java | 35 ++++++++++++++- .../RotationalPivot.java | 1 + .../RotationalPivotStates.java | 4 ++ 13 files changed, 155 insertions(+), 92 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 2c35fb8..3b25161 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -22,12 +22,14 @@ } ], "axisCount": 5, - "buttonCount": 4, + "buttonCount": 6, "buttonKeys": [ 90, 88, 67, - 86 + 86, + 66, + 78 ], "povConfig": [ { @@ -55,12 +57,14 @@ } ], "axisCount": 2, - "buttonCount": 4, + "buttonCount": 6, "buttonKeys": [ - 77, - 44, - 46, - 47 + 49, + 50, + 51, + 52, + 53, + 54 ], "povCount": 0 }, @@ -95,8 +99,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" }, { "guid": "Keyboard1" diff --git a/simgui-window.json b/simgui-window.json index 173abde..2842e9b 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -64,18 +64,23 @@ "###FMS": { "Collapsed": "0", "Pos": "268,27", - "Size": "210,214" + "Size": "202,214" }, "###Joysticks": { "Collapsed": "0", "Pos": "11,442", - "Size": "976,278" + "Size": "976,219" }, "###Keyboard 0 Settings": { "Collapsed": "0", "Pos": "266,326", "Size": "300,560" }, + "###Keyboard 1 Settings": { + "Collapsed": "0", + "Pos": "320,50", + "Size": "300,560" + }, "###NetworkTables": { "Collapsed": "0", "DockId": "0x00000001,0", diff --git a/src/main/java/frc/rebuilt/ShotCalculator.java b/src/main/java/frc/rebuilt/ShotCalculator.java index 11dc52b..9064435 100644 --- a/src/main/java/frc/rebuilt/ShotCalculator.java +++ b/src/main/java/frc/rebuilt/ShotCalculator.java @@ -87,7 +87,7 @@ public ShootingParameters getParameters() { } // Target location on the field - boolean feed = RobotStates.robotInFeedZone.getAsBoolean(); + boolean feed = RobotStates.robotInFeedZone.getAsBoolean() && !RobotStates.forceScore.getAsBoolean(); Translation2d target = feed ? FeedTargetFactory.generate() : HubTargetFactory.generate().toTranslation2d(); // Calculate estimated pose while accounting for phase delay diff --git a/src/main/java/frc/robot/Coordinator.java b/src/main/java/frc/robot/Coordinator.java index c1d0659..b7b2b5c 100644 --- a/src/main/java/frc/robot/Coordinator.java +++ b/src/main/java/frc/robot/Coordinator.java @@ -1,9 +1,12 @@ package frc.robot; +import org.checkerframework.checker.units.qual.C; + import frc.robot.fuelIntake.FuelIntakeStates; import frc.robot.indexer.IndexerStates; import frc.robot.intakeExtension.IntakeExtensionStates; import frc.robot.launcher.LauncherStates; +import frc.robot.swerve.SwerveStates; import frc.robot.turretRotationalPivot.RotationalPivotStates; public class Coordinator { @@ -17,44 +20,68 @@ public void applyRobotState(State state) { FuelIntakeStates.stop(); IndexerStates.neutral(); IntakeExtensionStates.fullExtend(); - LauncherStates.neutral(); RotationalPivotStates.neutral(); } case INTAKE_FUEL -> { FuelIntakeStates.intakeFuel(); IndexerStates.neutral(); IntakeExtensionStates.fullExtend(); - LauncherStates.neutral(); RotationalPivotStates.neutral(); } - case TURRET_TRACK_WITH_SPINUP -> { + case SNAKE_INTAKE -> { + FuelIntakeStates.intakeFuel(); + IndexerStates.neutral(); + IntakeExtensionStates.fullExtend(); + RotationalPivotStates.neutral(); + } + case TURRET_TRACK -> { FuelIntakeStates.stop(); IndexerStates.neutral(); IntakeExtensionStates.fullExtend(); - LauncherStates.aimAtHub(); RotationalPivotStates.aimAtHub(); } case TURRET_TRACK_WITH_LAUNCH -> { FuelIntakeStates.stop(); IndexerStates.indexMax(); IntakeExtensionStates.fullExtend(); - LauncherStates.aimAtHub(); RotationalPivotStates.aimAtHub(); } - case TURRET_FEED_WITH_SPINUP -> { + case TURRET_WITHOUT_TRACK -> { + FuelIntakeStates.stop(); + IndexerStates.neutral(); + IntakeExtensionStates.fullExtend(); + RotationalPivotStates.aimAtPresetPosition(); + } + case TURRET_WITHOUT_TRACK_WITH_LAUNCH -> { FuelIntakeStates.stop(); IndexerStates.indexMax(); IntakeExtensionStates.fullExtend(); - LauncherStates.aimAtHub(); + RotationalPivotStates.aimAtPresetPosition(); + } + case TURRET_FEED_WITH_AIMING -> { + FuelIntakeStates.stop(); + IndexerStates.neutral(); + IntakeExtensionStates.fullExtend(); RotationalPivotStates.aimAtHub(); } case TURRET_FEED_WITH_LAUNCH -> { FuelIntakeStates.stop(); IndexerStates.indexMax(); IntakeExtensionStates.fullExtend(); - LauncherStates.aimAtHub(); RotationalPivotStates.aimAtHub(); } + case TURRET_FEED_WITHOUT_AIMING -> { + FuelIntakeStates.stop(); + IndexerStates.neutral(); + IntakeExtensionStates.fullExtend(); + RotationalPivotStates.aimAtPresetPosition(); + } + case TURRET_FEED_WITHOUT_AIMING_WITH_LAUNCH -> { + FuelIntakeStates.stop(); + IndexerStates.indexMax(); + IntakeExtensionStates.fullExtend(); + RotationalPivotStates.aimAtPresetPosition(); + } case L1_CLIMB_PREP -> { } diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index eafd6b9..b5f08d4 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -5,6 +5,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.auton.Auton; +import frc.robot.launcher.LauncherStates; +import frc.robot.operator.Operator; import frc.robot.pilot.Pilot; import frc.robot.swerve.Swerve; import frc.spectrumLib.Telemetry; @@ -17,6 +19,7 @@ public class RobotStates { private static final Coordinator coordinator = Robot.getCoordinator(); private static final Pilot pilot = Robot.getPilot(); + private static final Operator operator = Robot.getOperator(); private static final Swerve swerve = Robot.getSwerve(); @Getter @@ -35,30 +38,46 @@ public class RobotStates { // Define triggers here public static final Trigger robotInNeutralZone = swerve.inNeutralZone(); public static final Trigger robotInEnemyZone = swerve.inEnemyAllianceZone(); + public static final Trigger forceScore = operator.AButton; public static final Trigger hopperFull = new Trigger(() -> true); - public static final Trigger robotReadyScore = new Trigger(() -> false); //movement stable + stable vision - public static final Trigger robotReadyFeed = new Trigger(hopperFull); //movement stable + vision stable + hopper full public static final Trigger robotInScoreZone = robotInEnemyZone.not().and(robotInNeutralZone.not()); - public static final Trigger robotInFeedZone = robotInEnemyZone.or(robotInNeutralZone); + public static final Trigger robotInFeedZone = robotInEnemyZone.or(robotInNeutralZone); + public static final Trigger robotReadyScore = (pilot.RB.or(pilot.XButton)).and(new Trigger(() -> true), robotInScoreZone); //movement stable + stable vision + public static final Trigger robotReadyFeed = pilot.RB.and(new Trigger(hopperFull), robotInFeedZone); //movement stable + vision stable + hopper full public static void setupStates() { // Pilot Triggers pilot.AButton.onTrue(applyState(State.INTAKE_FUEL)); pilot.AButton.onFalse(applyState(State.IDLE)); - pilot.BButton.onTrue(); - pilot.XButton.onTrue(applyState(State.TURRET_TRACK_WITH_SPINUP)); - pilot.YButton.onTrue(applyState(State.TURRET_FEED_WITH_SPINUP)); - pilot.home_select.onTrue(clearState()); - robotInScoreZone.and(robotReadyScore.not()).onTrue(applyState(State.TURRET_TRACK_WITH_SPINUP)); - robotInScoreZone.and(robotReadyScore).onTrue(applyState(State.TURRET_TRACK_WITH_LAUNCH)); + pilot.BButton.onTrue(applyState(State.SNAKE_INTAKE)); //snake intake toggle + pilot.BButton.onFalse(applyState(State.IDLE)); + + pilot.XButton.onTrue(applyState(State.TURRET_WITHOUT_TRACK)); + pilot.XButton.onFalse(applyState(State.IDLE)); + robotReadyScore.onTrue(applyState(State.TURRET_WITHOUT_TRACK_WITH_LAUNCH)); + robotReadyScore.onFalse(applyState(State.TURRET_WITHOUT_TRACK)); + + pilot.YButton.toggleOnTrue(LauncherStates.launchFuel()); - robotInFeedZone.and(robotReadyFeed.not()).onTrue(applyState(State.TURRET_FEED_WITH_SPINUP)); - robotInFeedZone.and(robotReadyFeed).onTrue(applyState(State.TURRET_FEED_WITH_LAUNCH)); + pilot.RB.and(robotInScoreZone).onTrue(applyState(State.TURRET_TRACK)); + pilot.RB.or(robotInScoreZone).onFalse(applyState(State.IDLE)); + robotReadyScore.onTrue(applyState(State.TURRET_TRACK_WITH_LAUNCH)); + robotReadyScore.onFalse(applyState(State.TURRET_TRACK)); + + pilot.RB.and(robotInFeedZone).onTrue(applyState(State.TURRET_FEED_WITH_AIMING)); + pilot.RB.or(robotInFeedZone).onFalse(applyState(State.IDLE)); + robotReadyFeed.onTrue(applyState(State.TURRET_FEED_WITH_LAUNCH)); + robotReadyFeed.onFalse(applyState(State.TURRET_FEED_WITH_AIMING)); + + pilot.startButton.onTrue(applyState(State.L1_CLIMB_PREP)); + pilot.RB.and(pilot.startButton).onTrue(applyState(State.L1_CLIMB_EXECUTE)); + + pilot.home_select.onTrue(clearState()); // Auton Triggers Auton.autonIntake.onTrue(applyState(State.INTAKE_FUEL)); - Auton.autonShotPrep.onTrue(applyState(State.TURRET_TRACK_WITH_SPINUP)); + Auton.autonShotPrep.onTrue(applyState(State.TURRET_TRACK)); Auton.autonShoot.onTrue(applyState(State.TURRET_TRACK_WITH_LAUNCH)); Auton.autonClearState.onTrue(clearState()); } diff --git a/src/main/java/frc/robot/State.java b/src/main/java/frc/robot/State.java index 138d2d3..abd1982 100644 --- a/src/main/java/frc/robot/State.java +++ b/src/main/java/frc/robot/State.java @@ -9,14 +9,20 @@ public enum State { IDLE, INTAKE_FUEL, - SMART_INTAKE, + SNAKE_INTAKE, - TURRET_TRACK_WITH_SPINUP, + TURRET_TRACK, TURRET_TRACK_WITH_LAUNCH, - TURRET_FEED_WITH_SPINUP, + TURRET_WITHOUT_TRACK, + TURRET_WITHOUT_TRACK_WITH_LAUNCH, + + TURRET_FEED_WITH_AIMING, TURRET_FEED_WITH_LAUNCH, + TURRET_FEED_WITHOUT_AIMING, + TURRET_FEED_WITHOUT_AIMING_WITH_LAUNCH, + L1_CLIMB_PREP, L1_CLIMB_EXECUTE, @@ -29,7 +35,7 @@ private State() { // Define the scoring sequence map, the 2nd state is the next state after the // current one private static final ImmutableMap scoreSequence = ImmutableMap.ofEntries( - Map.entry(TURRET_TRACK_WITH_SPINUP, TURRET_TRACK_WITH_LAUNCH)); + Map.entry(TURRET_TRACK, TURRET_TRACK_WITH_LAUNCH)); // ------ STATE ATTRIBUTES ------// @@ -39,7 +45,7 @@ public State getNextState(State state) { private static BooleanSupplier isReadyState(State state) { return () -> switch (state) { - case TURRET_TRACK_WITH_SPINUP -> true; + case TURRET_TRACK -> true; default -> false; }; } diff --git a/src/main/java/frc/robot/launcher/Launcher.java b/src/main/java/frc/robot/launcher/Launcher.java index c2ffc46..0d2a2a6 100644 --- a/src/main/java/frc/robot/launcher/Launcher.java +++ b/src/main/java/frc/robot/launcher/Launcher.java @@ -14,6 +14,7 @@ import java.util.function.DoubleSupplier; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.sim.TalonFXSimState; import lombok.Getter; @@ -36,8 +37,8 @@ public static class LauncherConfig extends Config { @Getter private double velocityKs = 14; /* Sim Configs */ - @Getter private double intakeX = Units.inchesToMeters(50); - @Getter private double intakeY = Units.inchesToMeters(63); + @Getter private double launcherX = Units.inchesToMeters(50); + @Getter private double launcherY = Units.inchesToMeters(63); @Getter private double wheelDiameter = 4; public LauncherConfig() { @@ -51,6 +52,7 @@ public LauncherConfig() { configReverseTorqueCurrentLimit(torqueCurrentLimit); configNeutralBrakeMode(true); configCounterClockwise_Positive(); + setFollowerConfigs(new FollowerConfig("LauncherFollower", 49, Rio.CANIVORE, MotorAlignmentValue.Opposed)); } } @@ -150,7 +152,7 @@ class LauncherSim extends RollerSim { public LauncherSim(Mechanism2d mech, TalonFXSimState rollerMotorSim) { super( new RollerConfig(config.getWheelDiameter()) - .setPosition(config.getIntakeX(), config.getIntakeY()), + .setPosition(config.getLauncherX(), config.getLauncherY()), mech, rollerMotorSim, config.getName()); diff --git a/src/main/java/frc/robot/launcher/LauncherStates.java b/src/main/java/frc/robot/launcher/LauncherStates.java index 765a5ef..5863e6c 100644 --- a/src/main/java/frc/robot/launcher/LauncherStates.java +++ b/src/main/java/frc/robot/launcher/LauncherStates.java @@ -6,33 +6,33 @@ import frc.spectrumLib.Telemetry; public class LauncherStates { - private static Launcher intake = Robot.getLauncher(); + private static Launcher launcher = Robot.getLauncher(); private static Launcher.LauncherConfig config = Robot.getConfig().launcher; public static void setupDefaultCommand() { - intake.setDefaultCommand( - intake.stopMotor().ignoringDisable(true).withName("Launcher.default")); + launcher.setDefaultCommand( + launcher.stopMotor().ignoringDisable(true).withName("Launcher.default")); } public static void neutral() { - scheduleIfNotRunning(intake.runVoltage(() -> 0).withName("Launcher.neutral")); + scheduleIfNotRunning(launcher.runVoltage(() -> 0).withName("Launcher.neutral")); } public static Command launchFuel() { - return intake.runTorqueFOC(config::getLauncherTorqueCurrent) + return launcher.runTorqueFOC(config::getLauncherTorqueCurrent) .withName("Launcher.launchFuelCommand"); } public static void coastMode() { - scheduleIfNotRunning(intake.coastMode()); + scheduleIfNotRunning(launcher.coastMode()); } public static void ensureBrakeMode() { - scheduleIfNotRunning(intake.ensureBrakeMode()); + scheduleIfNotRunning(launcher.ensureBrakeMode()); } public static void aimAtHub() { - scheduleIfNotRunning(intake.trackTargetCommand().withName("Launcher.aimAtHub")); + scheduleIfNotRunning(launcher.trackTargetCommand().withName("Launcher.aimAtHub")); } // Log Command @@ -49,7 +49,7 @@ public static void scheduleIfNotRunning(Command command) { CommandScheduler commandScheduler = CommandScheduler.getInstance(); // Check what command is currently requiring this subsystem - Command current = commandScheduler.requiring(intake); + Command current = commandScheduler.requiring(launcher); // Only schedule if it's not already the same same command if (current != command) { diff --git a/src/main/java/frc/robot/operator/Operator.java b/src/main/java/frc/robot/operator/Operator.java index 4656735..4c33f87 100644 --- a/src/main/java/frc/robot/operator/Operator.java +++ b/src/main/java/frc/robot/operator/Operator.java @@ -19,33 +19,11 @@ public class Operator extends Gamepad { public final Trigger noFn = fn.not(); public final Trigger home_select = select.or(leftStickClick); - public final Trigger climbPrep_start = start.and(noFn, enabled); - - public final Trigger coralStage = leftBumper.and(enabled); - public final Trigger algaeStage = rightBumper.and(enabled); - public final Trigger staged = coralStage.or(algaeStage); - public final Trigger nothingStaged = coralStage.not().and(algaeStage.not()); - - public final Trigger L1 = A.and(staged); - public final Trigger L2 = B.and(staged); - public final Trigger L3 = X.and(staged); - public final Trigger L4 = Y.and(staged); - - public final Trigger leftScore = leftDpad.and(staged); - public final Trigger rightScore = rightDpad.and(staged); - - public final Trigger latchOpen_startUp = climbPrep_start.and(upDpad); - public final Trigger latchCloser_startDown = climbPrep_start.and(downDpad); - - public final Trigger homeElevator_A = A.and(nothingStaged, teleop); - - public final Trigger toggleReverse = upDpad.and(climbPrep_start.not(), teleop); - - public final Trigger antiSecretClimb_LTRSup = - leftTriggerOnly.and(rightYTrigger(Threshold.LESS, -0.5)); - - public final Trigger processorScore_LT = leftTriggerOnly; - + public final Trigger AButton = A.and(teleop); + public final Trigger BButton = B.and(teleop); + public final Trigger XButton = X.and(teleop); + public final Trigger YButton = Y.and(teleop); + // DISABLED TRIGGERS public final Trigger coastOn_dB = disabled.and(B); public final Trigger coastOff_dA = disabled.and(A); diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index ffb4032..b81fbb4 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -267,13 +267,6 @@ public Trigger inEnemyAllianceZone() { }); } - public Trigger inFieldLeft() { - final double fieldWidthMeters = Units.feetToMeters(27.0); // full field width (Y) - final double halfWidth = fieldWidthMeters / 2.0; - - return new Trigger(() -> getRobotPose().getY() >= halfWidth); - } - public Trigger inFieldRight() { final double fieldWidthMeters = Units.feetToMeters(27.0); // full field width (Y) final double halfWidth = fieldWidthMeters / 2.0; @@ -288,14 +281,6 @@ public Trigger inFieldLeft() { return new Trigger(() -> getRobotPose().getY() >= halfWidth); } - public Trigger inFieldRight() { - final double fieldWidthMeters = Units.feetToMeters(27.0); // full field width (Y) - final double halfWidth = fieldWidthMeters / 2.0; - - return new Trigger(() -> getRobotPose().getY() < halfWidth); - } - - private static final double DEFAULT_OVERSPEED_METERS_PER_SECOND = 10.0; /** diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index b6cce45..7a602b1 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -9,14 +9,18 @@ import frc.rebuilt.Field; import frc.rebuilt.Zones; import frc.robot.Robot; +import frc.robot.RobotStates; +import frc.robot.operator.Operator; import frc.robot.pilot.Pilot; import frc.spectrumLib.Telemetry; +import java.util.Set; import java.util.function.DoubleSupplier; public class SwerveStates { static Swerve swerve = Robot.getSwerve(); static SwerveConfig config = Robot.getConfig().swerve; static Pilot pilot = Robot.getPilot(); + static Operator operator = Robot.getOperator(); static Zones zones = new Zones(); static Field field = new Field(); @@ -48,7 +52,9 @@ protected static void setStates() { swerve.getDefaultCommand()); pilot.fpv_LS.whileTrue(log(fpvDrive())); - pilot.AButton.whileTrue(log(snakeDrive())); + + pilot.BButton.toggleOnTrue(log(snakeDrive())); + RobotStates.robotReadyScore.or(RobotStates.robotReadyFeed).toggleOnTrue(log(tweakOut())); pilot.upReorient.onTrue(log(reorientForward())); pilot.leftReorient.onTrue(log(reorientLeft())); @@ -117,6 +123,33 @@ protected static Command snakeDrive() { pilot::getPilotStickAngle) .withName("Swerve.SnakeDrive"); } + protected static Command tweakOut() { + return Commands.runOnce(() -> swerve.resetRotationController()) + .andThen( + Commands.defer( + () -> { + final double base = swerve.getRotation().getRadians(); + final double delta = Math.toRadians(15.0); + + Command toMinus = + drive( + pilot::getDriveFwdPositive, + pilot::getDriveLeftPositive, + getAlignHeading(() -> base - delta, false)) + .withTimeout(0.12); + + Command toPlus = + drive( + pilot::getDriveFwdPositive, + pilot::getDriveLeftPositive, + getAlignHeading(() -> base + delta, false)) + .withTimeout(0.12); + + return Commands.repeatingSequence(toMinus, toPlus); + }, + Set.of(swerve)) + .withName("Swerve.tweakOut")); + } /** Turn the swerve wheels to an X to prevent the robot from moving */ protected static Command xBrake() { diff --git a/src/main/java/frc/robot/turretRotationalPivot/RotationalPivot.java b/src/main/java/frc/robot/turretRotationalPivot/RotationalPivot.java index 3e3e8f1..b798c6a 100644 --- a/src/main/java/frc/robot/turretRotationalPivot/RotationalPivot.java +++ b/src/main/java/frc/robot/turretRotationalPivot/RotationalPivot.java @@ -31,6 +31,7 @@ public static class RotationalPivotConfig extends Config { @Getter @Setter private boolean reversed = false; @Getter private final double initPosition = 0; + @Getter private final double presetPosition = 90; @Getter private double triggerTolerance = 5; @Getter private double unwrapTolerance = 10; diff --git a/src/main/java/frc/robot/turretRotationalPivot/RotationalPivotStates.java b/src/main/java/frc/robot/turretRotationalPivot/RotationalPivotStates.java index 3a90aff..812a0f4 100644 --- a/src/main/java/frc/robot/turretRotationalPivot/RotationalPivotStates.java +++ b/src/main/java/frc/robot/turretRotationalPivot/RotationalPivotStates.java @@ -20,6 +20,10 @@ public static void aimAtHub() { scheduleIfNotRunning(log(turretRotation.trackTargetCommand()).withName("Turret.aimAtHub")); } + public static void aimAtPresetPosition() { + scheduleIfNotRunning(log(turretRotation.moveToDegrees(config::getPresetPosition)).withName("Turret.aimAtPreset")); + } + public static void neutral() { scheduleIfNotRunning(turretRotation.runVoltage(() -> 0).withName("Turret.neutral")); } From 4c719c45bdf7f6c2c64d0c644f2b4d7136f30d9c Mon Sep 17 00:00:00 2001 From: Spectrum Date: Sun, 8 Feb 2026 20:03:21 -0600 Subject: [PATCH 3/3] more miscellaneous changes --- simgui-window.json | 17 ++- simgui.json | 12 +- src/main/java/frc/robot/Coordinator.java | 2 + src/main/java/frc/robot/RobotStates.java | 136 +++++++++++------- .../java/frc/robot/swerve/SwerveStates.java | 10 +- 5 files changed, 117 insertions(+), 60 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index 2842e9b..2da5811 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -1,7 +1,7 @@ { "Docking": { "Data": [ - "DockNode ID=0x00000001 Pos=9,835 Size=937,509 Selected=0x48913727" + "DockNode ID=0x00000001 Pos=9,835 Size=937,509 Selected=0x2D9B9756" ] }, "MainWindow": { @@ -51,6 +51,21 @@ "Pos": "1002,1036", "Size": "340,360" }, + "###/SmartDashboard/Sim/LeftView": { + "Collapsed": "0", + "Pos": "1366,653", + "Size": "393,372" + }, + "###/SmartDashboard/Sim/TopView": { + "Collapsed": "0", + "Pos": "998,654", + "Size": "361,374" + }, + "###/SmartDashboard/States": { + "Collapsed": "0", + "Pos": "247,711", + "Size": "300,150" + }, "###/SmartDashboard/TopView": { "Collapsed": "0", "Pos": "998,652", diff --git a/simgui.json b/simgui.json index 66a35aa..ebc082e 100644 --- a/simgui.json +++ b/simgui.json @@ -44,17 +44,17 @@ "visible": true } }, - "/SmartDashboard/LeftView": { + "/SmartDashboard/Scheduler": { "window": { "visible": true } }, - "/SmartDashboard/Scheduler": { + "/SmartDashboard/Sim/LeftView": { "window": { "visible": true } }, - "/SmartDashboard/TopView": { + "/SmartDashboard/Sim/TopView": { "window": { "visible": true } @@ -89,6 +89,9 @@ } }, "NetworkTables": { + "Retained Values": { + "open": false + }, "transitory": { "Robot": { "FieldSimulation": { @@ -106,6 +109,9 @@ } }, "NetworkTables Info": { + "Clients": { + "open": true + }, "visible": true } } diff --git a/src/main/java/frc/robot/Coordinator.java b/src/main/java/frc/robot/Coordinator.java index b7b2b5c..63b9b1c 100644 --- a/src/main/java/frc/robot/Coordinator.java +++ b/src/main/java/frc/robot/Coordinator.java @@ -2,6 +2,8 @@ import org.checkerframework.checker.units.qual.C; +import edu.wpi.first.networktables.NTSendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.fuelIntake.FuelIntakeStates; import frc.robot.indexer.IndexerStates; import frc.robot.intakeExtension.IntakeExtensionStates; diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index b5f08d4..4c958ce 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -14,96 +14,124 @@ /** * Manages the high-level robot states. - * This class coordinates multiple subsystems based on the current robot state. + * Coordinates multiple subsystems based on the current robot state. */ -public class RobotStates { +public final class RobotStates { private static final Coordinator coordinator = Robot.getCoordinator(); private static final Pilot pilot = Robot.getPilot(); private static final Operator operator = Robot.getOperator(); private static final Swerve swerve = Robot.getSwerve(); - @Getter - private static State appliedState = State.IDLE; + @Getter public static State appliedState = State.IDLE; - /** - * Define Robot States here and how they can be triggered States should be - * triggers that command - * multiple mechanism or can be used in teleop or auton Use onTrue/whileTrue to - * run a command - * when entering the state Use onFalse/whileFalse to run a command when leaving - * the state - * RobotType Triggers - */ - - // Define triggers here 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 robotInScoreZone = robotInEnemyZone.not().and(robotInNeutralZone.not()); - public static final Trigger robotInFeedZone = robotInEnemyZone.or(robotInNeutralZone); - public static final Trigger robotReadyScore = (pilot.RB.or(pilot.XButton)).and(new Trigger(() -> true), robotInScoreZone); //movement stable + stable vision - public static final Trigger robotReadyFeed = pilot.RB.and(new Trigger(hopperFull), robotInFeedZone); //movement stable + vision stable + hopper full - - public static void setupStates() { - // Pilot Triggers - pilot.AButton.onTrue(applyState(State.INTAKE_FUEL)); - pilot.AButton.onFalse(applyState(State.IDLE)); - pilot.BButton.onTrue(applyState(State.SNAKE_INTAKE)); //snake intake toggle - pilot.BButton.onFalse(applyState(State.IDLE)); + //placeholders + private static final Trigger movementStable = new Trigger(() -> true); + private static final Trigger visionStable = new Trigger(() -> true); + + public static final Trigger robotReadyScore = (robotInScoreZone).and(movementStable).and(visionStable); - pilot.XButton.onTrue(applyState(State.TURRET_WITHOUT_TRACK)); - pilot.XButton.onFalse(applyState(State.IDLE)); - robotReadyScore.onTrue(applyState(State.TURRET_WITHOUT_TRACK_WITH_LAUNCH)); - robotReadyScore.onFalse(applyState(State.TURRET_WITHOUT_TRACK)); + public static final Trigger robotReadyFeed = (robotInFeedZone).and(hopperFull).and(movementStable).and(visionStable); + + private RobotStates() { + throw new IllegalStateException("Utility class"); + } + + public static void setupStates() { + pressToState(pilot.AButton, State.INTAKE_FUEL); + toggleToState(pilot.BButton, State.SNAKE_INTAKE); + + bindAimingWithReadyUpgrade( + pilot.XButton, + robotInScoreZone, + State.TURRET_WITHOUT_TRACK, + robotReadyScore, + State.TURRET_WITHOUT_TRACK_WITH_LAUNCH); pilot.YButton.toggleOnTrue(LauncherStates.launchFuel()); - - pilot.RB.and(robotInScoreZone).onTrue(applyState(State.TURRET_TRACK)); - pilot.RB.or(robotInScoreZone).onFalse(applyState(State.IDLE)); - robotReadyScore.onTrue(applyState(State.TURRET_TRACK_WITH_LAUNCH)); - robotReadyScore.onFalse(applyState(State.TURRET_TRACK)); - pilot.RB.and(robotInFeedZone).onTrue(applyState(State.TURRET_FEED_WITH_AIMING)); - pilot.RB.or(robotInFeedZone).onFalse(applyState(State.IDLE)); - robotReadyFeed.onTrue(applyState(State.TURRET_FEED_WITH_LAUNCH)); - robotReadyFeed.onFalse(applyState(State.TURRET_FEED_WITH_AIMING)); + bindAimingWithReadyUpgrade( + pilot.RB, + robotInScoreZone, + State.TURRET_TRACK, + robotReadyScore, + State.TURRET_TRACK_WITH_LAUNCH); + + bindAimingWithReadyUpgrade( + pilot.RB, + robotInFeedZone, + State.TURRET_FEED_WITH_AIMING, + robotReadyFeed, + State.TURRET_FEED_WITH_LAUNCH); + // Climb pilot.startButton.onTrue(applyState(State.L1_CLIMB_PREP)); pilot.RB.and(pilot.startButton).onTrue(applyState(State.L1_CLIMB_EXECUTE)); + // Clear pilot.home_select.onTrue(clearState()); - // Auton Triggers + // Auton Auton.autonIntake.onTrue(applyState(State.INTAKE_FUEL)); Auton.autonShotPrep.onTrue(applyState(State.TURRET_TRACK)); Auton.autonShoot.onTrue(applyState(State.TURRET_TRACK_WITH_LAUNCH)); Auton.autonClearState.onTrue(clearState()); } - private RobotStates() { - throw new IllegalStateException("Utility class"); + private static void toggleToState(Trigger button, State toggledState) { + button.onTrue( + new InstantCommand( + () -> { + State next = (appliedState == toggledState) ? State.IDLE : toggledState; + appliedState = next; + applyState(next); + }) + ); + } + + private static void pressToState(Trigger button, State pressedState) { + button.onTrue(applyState(pressedState)); + button.onFalse(applyState(State.IDLE)); + } + + private static void bindAimingWithReadyUpgrade(Trigger button, Trigger zone, State aimingState, Trigger readyTrigger, State readyState) { + Trigger active = button.and(zone); + + active.onTrue(applyState(aimingState)); + active.onFalse(applyState(State.IDLE)); + + active.and(readyTrigger).onTrue(applyState(readyState)); + active.and(readyTrigger.not()).onTrue(applyState(aimingState)); } public static Command applyState(State state) { return new InstantCommand( - () -> { - appliedState = state; - SmartDashboard.putString("APPLIED STATE", state.toString()); - Telemetry.print("Applied State: " + state.toString()); - coordinator.applyRobotState(state); - }) - .withName("APPLYING STATE: " + state.toString()); + () -> { + appliedState = state; + SmartDashboard.putString("APPLIED STATE", state.toString()); + SmartDashboard.putString("Current Zone", robotInNeutralZone.getAsBoolean() ? "Neutral" : robotInEnemyZone.getAsBoolean() ? "Enemy" : "Alliance"); + Telemetry.print("Applied State: " + state); + coordinator.applyRobotState(state); + }, + swerve) + .withName("APPLYING STATE: " + state); } public static Command clearState() { return new InstantCommand( - () -> { - appliedState = State.IDLE; - SmartDashboard.putString("APPLIED STATE", "CLEARED TO IDLE"); - coordinator.applyRobotState(State.IDLE); - }) + () -> { + appliedState = State.IDLE; + SmartDashboard.putString("APPLIED STATE", "CLEARED TO IDLE"); + coordinator.applyRobotState(State.IDLE); + }, + swerve) .withName("CLEARING STATE TO IDLE"); } } diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index 7a602b1..7e9cc88 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -6,10 +6,12 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.rebuilt.Field; import frc.rebuilt.Zones; import frc.robot.Robot; import frc.robot.RobotStates; +import frc.robot.State; import frc.robot.operator.Operator; import frc.robot.pilot.Pilot; import frc.spectrumLib.Telemetry; @@ -46,6 +48,10 @@ protected static void setupDefaultCommand() { swerve.setDefaultCommand(pilotSteerCommand); } + //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); + protected static void setStates() { // Force back to manual steering when we steer pilot.steer.whileTrue( @@ -53,8 +59,8 @@ protected static void setStates() { pilot.fpv_LS.whileTrue(log(fpvDrive())); - pilot.BButton.toggleOnTrue(log(snakeDrive())); - RobotStates.robotReadyScore.or(RobotStates.robotReadyFeed).toggleOnTrue(log(tweakOut())); + inSnakeDrive.whileTrue(log(snakeDrive())); + inScoreOrFeed.whileTrue(log(tweakOut())); pilot.upReorient.onTrue(log(reorientForward())); pilot.leftReorient.onTrue(log(reorientLeft()));