Fix many state machine issues

FRCTeam3044 · Feb 16, 2025 · 0ed258c · 0ed258c
1 parent 62c262a
commit 0ed258c
Show file tree

Hide file tree

Showing 18 changed files with 126 additions and 59 deletions.
diff --git a/algae.png b/algae.png
diff --git a/coral.png b/coral.png
diff --git a/note.png b/note.png
diff --git a/simgui.json b/simgui.json
@@ -32,12 +32,50 @@
     },
     "windows": {
       "/SmartDashboard/Field": {
+        "Blocker": {
+          "arrows": false,
+          "color": [
+            0.24313725531101227,
+            0.24313725531101227,
+            0.24313725531101227,
+            255.0
+          ],
+          "length": 0.20999999344348907,
+          "weight": 25.0,
+          "width": 0.20999999344348907
+        },
         "Path": {
           "arrows": false,
           "style": "Line"
         },
+        "Robot": {
+          "arrowWeight": 3.0,
+          "length": 0.9200000166893005,
+          "selectable": false,
+          "weight": 3.0,
+          "width": 0.9200000166893005
+        },
+        "algae": {
+          "arrows": false,
+          "image": "/home/nicholas/Documents/coding/robotics/2025/2025swervebase/algae.png",
+          "length": 0.3499999940395355,
+          "width": 0.3499999940395355
+        },
         "bottom": 1638,
         "builtin": "2025 Reefscape",
+        "coral": {
+          "arrows": false,
+          "color": [
+            1.0,
+            1.0,
+            1.0,
+            255.0
+          ],
+          "image": "/home/nicholas/Documents/coding/robotics/2025/2025swervebase/coral.png",
+          "length": 0.30000001192092896,
+          "weight": 0.0,
+          "width": 0.10999999940395355
+        },
         "height": 8.051901817321777,
         "left": 534,
         "right": 3466,

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -16,7 +16,6 @@
 import static frc.robot.subsystems.vision.VisionConstants.*;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
@@ -102,7 +101,6 @@ public class RobotContainer {
         private final MechanismRoot2d root;
         private final MechanismLigament2d elevatorSim;
         private final MechanismLigament2d shoulderSim;
-        private final MechanismLigament2d endEffectorSim;
 
         /**
          * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -150,7 +148,7 @@ public RobotContainer() {
                                 vision.setPoseSupplier(driveSimulation::getSimulatedDriveTrainPose);
                                 elevator = new Elevator(new ElevatorIOSim());
                                 shoulder = new Shoulder(new ShoulderIOSim());
-                                endEffector = new EndEffector(new EndEffectorIOSim());
+                                endEffector = new EndEffector(new EndEffectorIOSim(driverController.getHID()));
                                 buttonBoard = new ButtonBoard(Constants.simButtonBoard ? new BBoardIOSim() : new BBoardIOReal());
                                 LEDs = new LEDs(new LEDsIORio());
                                 break;
@@ -235,7 +233,7 @@ public RobotContainer() {
                 shoulderSim = elevatorSim
                                 .append(new MechanismLigament2d("shoulder", 0.65, shoulder.getShoulderAngle(), 6.0,
                                                 new Color8Bit(Color.kPurple)));
-                endEffectorSim = shoulderSim.append(
+                shoulderSim.append(
                                 new MechanismLigament2d("endEffector", 0.5, 90.0, 6.0, new Color8Bit(Color.kPurple)));
         }
 
@@ -264,20 +262,27 @@ public void displaySimFieldToAdvantageScope() {
                         return;
                 Logger.recordOutput("FieldSimulation/RobotPosition", driveSimulation.getSimulatedDriveTrainPose());
                 Logger.recordOutput(
-                                "FieldSimulation/Notes",
-                                SimulatedArena.getInstance().getGamePiecesByType("Note").toArray(new Pose3d[0]));
+                        "FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral"));
+                Logger.recordOutput(
+                        "FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae"));
         }
 
         public void displaySimFieldToSmartDashboard() {
                 if (Constants.currentMode != Constants.Mode.SIM)
                         return;
 
                 fieldSim.setRobotPose(driveSimulation.getSimulatedDriveTrainPose());
-                fieldSim.getObject("notes")
-                                .setPoses(SimulatedArena.getInstance().getGamePiecesByType("Note").stream().map((p) -> {
+                fieldSim.getObject("algae")
+                                .setPoses(SimulatedArena.getInstance().getGamePiecesByType("Algae").stream().map((p) -> {
                                         return new Pose2d(p.getTranslation().getX(), p.getTranslation().getY(),
                                                         new Rotation2d());
                                 }).toArray(Pose2d[]::new));
+                fieldSim.getObject("coral")
+                                .setPoses(SimulatedArena.getInstance().getGamePiecesByType("Coral").stream().map((p) -> {
+                                        return new Pose2d(p.getTranslation().getX(), p.getTranslation().getY(),
+                                                        new Rotation2d(p.getRotation().getZ()));
+                                }).toArray(Pose2d[]::new));
+
                 SmartDashboard.putData("Field", fieldSim);
         }
 

diff --git a/src/main/java/frc/robot/statemachine/StateMachine.java b/src/main/java/frc/robot/statemachine/StateMachine.java
@@ -102,11 +102,13 @@ public StateMachine(CommandXboxController driverController, CommandXboxControlle
                 // Example, will be button board later
                 manual.withTransition(goToScoringPosition,
                                 () -> driverController.rightTrigger().getAsBoolean()
-                                                && (endEffector.hasCoral() || endEffector.hasAlgae()),
+                                                && (endEffector.hasCoral() || endEffector.hasAlgae())
+                                                && buttonBoard.scoringSelected(),
                                 "Driver presses score")
                                 .withTransition(goToIntake,
                                                 () -> driverController.leftTrigger().getAsBoolean()
-                                                                && (!endEffector.hasCoral() && !endEffector.hasAlgae()),
+                                                                && (!endEffector.hasCoral() && !endEffector.hasAlgae())
+                                                                && buttonBoard.intakeSelected(),
                                                 "Driver presses intake");
 
                 goToScoringPosition

diff --git a/src/main/java/frc/robot/statemachine/reusable/State.java b/src/main/java/frc/robot/statemachine/reusable/State.java
@@ -12,6 +12,7 @@
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
  * A state in a state machine.
@@ -232,7 +233,7 @@ public boolean is(State state) {
      */
     public void onExit() {
         loop.stop();
-        currentStartCommands.forEach(Command::cancel);
+        CommandScheduler.getInstance().cancel(currentStartCommands.toArray(new Command[0]));
         currentStartCommands.clear();
     }
 

diff --git a/src/main/java/frc/robot/statemachine/states/TestState.java b/src/main/java/frc/robot/statemachine/states/TestState.java
@@ -17,7 +17,6 @@ public class TestState extends State {
         public TestState(StateMachineBase stateMachine, CommandXboxController controller, Elevator elevator,
                         Shoulder shoulder, EndEffector endEffector, LEDs LEDs) {
                 super(stateMachine);
-                @SuppressWarnings("unused")
                 SmartXboxController testController = new SmartXboxController(controller, loop);
 
                 DoubleSupplier rightY = () -> -MathUtil.applyDeadband(controller.getRightY(), 0.01);
@@ -26,7 +25,7 @@ public TestState(StateMachineBase stateMachine, CommandXboxController controller
                 startWhenActive(LEDs.simMorseCode());
                 startWhenActive(elevator.elevatorMove(rightY));
                 startWhenActive(shoulder.manualPivot(leftY));
-                controller.a().onTrue(endEffector.runIntake());
-                controller.b().onTrue(endEffector.runIntakeReverse());
+                testController.a().onTrue(endEffector.runIntake());
+                testController.b().onTrue(endEffector.runIntakeReverse());
         }
 }
diff --git a/src/main/java/frc/robot/statemachine/states/tele/ManualState.java b/src/main/java/frc/robot/statemachine/states/tele/ManualState.java
@@ -3,8 +3,6 @@
 import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -52,11 +50,9 @@ public ManualState(StateMachineBase stateMachine, CommandXboxController driver,
                 // Switch to X pattern when X button is pressed
                 driverController.x().onTrue(Commands.runOnce(drive::stopWithX, drive).withName("X mode"));
                 driverController.x().onFalse(joystickDrive);
-                driverController.a().onTrue(
-                                DriveCommands.goToPointJoystickRot(drive, new Pose2d(3, 3, new Rotation2d()), rotVel));
 
-                operatorController.leftTrigger().onTrue(endEffector.runIntakeReverse());
-                operatorController.rightTrigger().onTrue(endEffector.runIntake());
+                operatorController.leftTrigger().whileTrue(endEffector.runIntakeReverse());
+                operatorController.rightTrigger().whileTrue(endEffector.runIntake());
 
                 DoubleSupplier rightY = () -> -MathUtil.applyDeadband(operatorController.getHID().getRightY(), 0.01);
                 DoubleSupplier leftY = () -> -MathUtil.applyDeadband(operatorController.getHID().getLeftY(), 0.05);

diff --git a/src/main/java/frc/robot/statemachine/states/tele/ScoreCoral.java b/src/main/java/frc/robot/statemachine/states/tele/ScoreCoral.java
@@ -1,5 +1,7 @@
 package frc.robot.statemachine.states.tele;
 
+import java.util.Set;
+
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.statemachine.reusable.State;
 import frc.robot.statemachine.reusable.StateMachineBase;
@@ -12,16 +14,20 @@
 import frc.robot.util.bboard.ButtonBoard;
 
 public class ScoreCoral extends State {
-    public ScoreCoral(StateMachineBase stateMachine, ButtonBoard buttonBoard, Drive drive, EndEffector endEffector,
-            Elevator elevator, Shoulder shoulder, LEDs LEDs) {
-        super(stateMachine);
+        public ScoreCoral(StateMachineBase stateMachine, ButtonBoard buttonBoard, Drive drive, EndEffector endEffector,
+                        Elevator elevator, Shoulder shoulder, LEDs LEDs) {
+                super(stateMachine);
 
-        startWhenActive(DriveCommands.pointControl(drive, buttonBoard::getCoralReefTarget));
-        startWhenActive(Commands.deferredProxy(
-                () -> elevator.toCoral(buttonBoard.getCoralReefLevel(), buttonBoard.getCoralReefTargetDist(drive))));
-        startWhenActive(Commands.deferredProxy(
-                () -> shoulder.scoreCoral(buttonBoard.getCoralReefLevel(), buttonBoard.getCoralReefTargetDist(drive))));
-        startWhenActive(endEffector.runIntakeReverse());
-        startWhenActive(LEDs.intakingAndScoringCoral());
-    }
+                startWhenActive(DriveCommands.pointControl(drive, buttonBoard::getCoralReefTarget));
+                startWhenActive(Commands
+                                .defer(() -> elevator.toCoral(buttonBoard.getCoralReefLevel(),
+                                                buttonBoard.getCoralReefTargetDist(drive)), Set.of(elevator))
+                                .withName("Elevator to CoralLevel"));
+                startWhenActive(Commands
+                                .defer(() -> shoulder.scoreCoral(buttonBoard.getCoralReefLevel(),
+                                                buttonBoard.getCoralReefTargetDist(drive)), Set.of(shoulder))
+                                .withName("Shoulder to CoralLevel"));
+                startWhenActive(endEffector.runIntakeReverse());
+                startWhenActive(LEDs.intakingAndScoringCoral());
+        }
 }
diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java
@@ -25,15 +25,15 @@ public void periodic() {
     }
 
     public Command runIntakeSpeed(DoubleSupplier desiredSpeed) {
-        return Commands.runEnd(() -> io.setCoralSpeed(desiredSpeed.getAsDouble()), () -> io.setCoralSpeed(0.0));
+        return Commands.runEnd(() -> io.setCoralSpeed(desiredSpeed.getAsDouble()), () -> io.setCoralSpeed(0.0), this).withName("Run Intake Speed");
     }
 
     public Command runIntake() {
-        return runIntakeSpeed(intakeSpeed::get);
+        return runIntakeSpeed(intakeSpeed::get).withName("Run Intake");
     }
 
     public Command runIntakeReverse() {
-        return runIntakeSpeed(() -> -intakeSpeed.get());
+        return runIntakeSpeed(() -> -intakeSpeed.get()).withName("Run Intake Reverse");
     }
 
     public boolean hasCoral() {

diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIOSim.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIOSim.java
@@ -1,21 +1,30 @@
 package frc.robot.subsystems.EndEffector;
 
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class EndEffectorIOSim implements EndEffectorIO {
+    private XboxController controller;
 
-    public EndEffectorIOSim() {
+    public EndEffectorIOSim(XboxController controller) {
+        this.controller = controller;
         SmartDashboard.putBoolean("HasCoral", false);
         SmartDashboard.putBoolean("HasAlgae", false);
     }
 
     @Override
     public boolean hasCoral() {
+        if(controller.getAButtonPressed()) {
+            SmartDashboard.putBoolean("HasCoral", !SmartDashboard.getBoolean("HasCoral", false));
+        }
         return SmartDashboard.getBoolean("HasCoral", false);
     }
 
     @Override
     public boolean hasAlgae() {
+        if(controller.getBButtonPressed()) {
+            SmartDashboard.putBoolean("HasAlgae", !SmartDashboard.getBoolean("HasAlgae", false));
+        }
         return SmartDashboard.getBoolean("HasAlgae", false);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/LEDs/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs/LEDs.java
@@ -42,7 +42,7 @@ public Command aprilTagDetected() {
     }
 
     public Command setBlinkingColor(Color color) {
-        return Commands.run(() -> io.setBlinkingColor(color));
+        return Commands.run(() -> io.setBlinkingColor(color)).withName("Blinking LEDs");
     }
 
     public Command intakingAndScoringCoral() {

diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java
@@ -23,6 +23,6 @@ public void periodic() {
     }
 
     public Command move(DoubleSupplier speed) {
-        return Commands.runEnd(() -> io.setSpeed(speed.getAsDouble()), () -> io.setSpeed(0.0));
+        return Commands.runEnd(() -> io.setSpeed(speed.getAsDouble()), () -> io.setSpeed(0.0), this);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java
@@ -39,7 +39,6 @@
 
 import java.text.DecimalFormat;
 import java.text.NumberFormat;
-import java.util.HashSet;
 import java.util.LinkedList;
 import java.util.List;
 import java.util.function.DoubleSupplier;
@@ -188,28 +187,23 @@ private static Command followTrajectory(Drive drive, Trajectory traj, Supplier<R
         }).until(() -> timer.hasElapsed(traj.getTotalTimeSeconds()))
                 .finallyDo(timer::stop).withName("Follow Trajectory");
     };
-    // change stop
 
     private static Command goToPoint(Drive drive, Pose2d pose, Supplier<Rotation2d> desiredRotation,
             DoubleSupplier joystickRot, boolean useJoystick) {
-        return Commands.defer(
-                () -> followTrajectory(drive, generateTrajectory(drive, pose), desiredRotation, joystickRot,
-                        useJoystick),
-                new HashSet<>());
+        return Commands.deferredProxy(() -> followTrajectory(drive, generateTrajectory(drive, pose), desiredRotation,
+                joystickRot, useJoystick)).withName("Go To Point");
     }
 
     public static Command goToPointJoystickRot(Drive drive, Pose2d pose, DoubleSupplier joystickRot) {
         return goToPoint(drive, pose, null, joystickRot, true).withName("Go To Point Joystick Rot");
     }
 
     public static Command goToPoint(Drive drive, Supplier<Pose2d> pose) {
-        return Commands.defer(
-                () -> {
-                    Pose2d curPose = pose.get();
-                    return followTrajectory(drive, generateTrajectory(drive, curPose), () -> curPose.getRotation(),
-                            null, false);
-                },
-                new HashSet<>());
+        return Commands.deferredProxy(() -> {
+            Pose2d curPose = pose.get();
+            return followTrajectory(drive, generateTrajectory(drive, curPose), () -> curPose.getRotation(),
+                    null, false);
+        }).withName("Go To Point");
     }
 
     public static Command pointControl(Drive drive, Supplier<Pose2d> pose) {

diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
@@ -51,13 +51,13 @@ public void periodic() {
     }
 
     public Command elevatorMove(DoubleSupplier speed) {
-        return Commands.runEnd(() -> io.setSpeed(speed.getAsDouble()), () -> io.setSpeed(0.0))
+        return Commands.runEnd(() -> io.setSpeed(speed.getAsDouble()), () -> io.setSpeed(0.0), this)
                 .withName("Elevator manual move");
     }
 
     public Command toCoral(CoralLevel level, DoubleSupplier robotDistance) {
-        return Commands.run(() -> io.setPosition(getHeightForCoral(level, robotDistance.getAsDouble())))
-                .withName("Set elevator to L1 Scoring level");
+        return Commands.run(() -> io.setPosition(getHeightForCoral(level, robotDistance.getAsDouble())), this)
+                .withName("Elevator to CoralLevel");
     }
 
     private double getHeightForCoral(CoralLevel level, double distance) {

diff --git a/src/main/java/frc/robot/subsystems/shoulder/Shoulder.java b/src/main/java/frc/robot/subsystems/shoulder/Shoulder.java
@@ -51,14 +51,15 @@ public void periodic() {
     }
 
     public Command manualPivot(DoubleSupplier desiredSpeed) {
-        return Commands.runEnd(() -> io.setShoulderSpeed(desiredSpeed.getAsDouble()), () -> io.setShoulderSpeed(0.0))
+        return Commands
+                .runEnd(() -> io.setShoulderSpeed(desiredSpeed.getAsDouble()), () -> io.setShoulderSpeed(0.0), this)
                 .withName("Shoulder Manual Pivot");
     }
 
     public Command scoreCoral(CoralLevel level, DoubleSupplier robotDistance) {
         return Commands
                 .run(() -> io.setShoulderAngle(calculateAngleForCoral(level, robotDistance.getAsDouble())), this)
-                .withName("Set Shoulder to L1 Scoring position");
+                .withName("Shoulder to CoralLevel");
     }
 
     private double calculateAngleForCoral(CoralLevel level, double robotDist) {