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..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", @@ -64,18 +79,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/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/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 ac82734..63b9b1c 100644 --- a/src/main/java/frc/robot/Coordinator.java +++ b/src/main/java/frc/robot/Coordinator.java @@ -1,9 +1,14 @@ package frc.robot; +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; import frc.robot.launcher.LauncherStates; +import frc.robot.swerve.SwerveStates; import frc.robot.turretRotationalPivot.RotationalPivotStates; public class Coordinator { @@ -17,44 +22,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(); + RotationalPivotStates.aimAtPresetPosition(); + } + case TURRET_FEED_WITH_AIMING -> { FuelIntakeStates.stop(); IndexerStates.neutral(); IntakeExtensionStates.fullExtend(); - LauncherStates.aimAtHub(); 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 30866e0..4c958ce 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; @@ -12,72 +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 hopperFull = new Trigger(null); + public static final Trigger robotInScoreZone = robotInFeedZone.not(); + + public static final Trigger forceScore = operator.AButton; + public static final Trigger hopperFull = new Trigger(() -> true); + + //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); + + public static final Trigger robotReadyFeed = (robotInFeedZone).and(hopperFull).and(movementStable).and(visionStable); + + private RobotStates() { + throw new IllegalStateException("Utility class"); + } - // 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.home_select.onTrue(clearState()); + 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()); - robotInNeutralZone.or(robotInEnemyZone).whileTrue(applyState(State.TURRET_FEED_WITH_SPINUP)); + bindAimingWithReadyUpgrade( + pilot.RB, + robotInScoreZone, + State.TURRET_TRACK, + robotReadyScore, + State.TURRET_TRACK_WITH_LAUNCH); - // Auton Triggers + 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 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()); } - 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/State.java b/src/main/java/frc/robot/State.java index 148af26..abd1982 100644 --- a/src/main/java/frc/robot/State.java +++ b/src/main/java/frc/robot/State.java @@ -9,13 +9,20 @@ public enum State { IDLE, INTAKE_FUEL, + 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, @@ -28,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 ------// @@ -38,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/pilot/Pilot.java b/src/main/java/frc/robot/pilot/Pilot.java index 7643a2a..908567e 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); @@ -39,12 +41,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 fa4845c..b81fbb4 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -267,20 +267,48 @@ public Trigger inEnemyAllianceZone() { }); } - public Trigger inFieldLeft() { + 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); + return new Trigger(() -> getRobotPose().getY() < halfWidth); } - public Trigger inFieldRight() { + 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); + 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) { + 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 diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index b6cce45..7e9cc88 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -6,17 +6,23 @@ 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; +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(); @@ -42,13 +48,19 @@ 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( swerve.getDefaultCommand()); pilot.fpv_LS.whileTrue(log(fpvDrive())); - pilot.AButton.whileTrue(log(snakeDrive())); + + inSnakeDrive.whileTrue(log(snakeDrive())); + inScoreOrFeed.whileTrue(log(tweakOut())); pilot.upReorient.onTrue(log(reorientForward())); pilot.leftReorient.onTrue(log(reorientLeft())); @@ -117,6 +129,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")); }