From 6e56e70df6dcaab56bad6337732cabcf0919f9ac Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Tue, 11 Mar 2025 16:47:40 -0500 Subject: [PATCH 01/19] temp changes, hasCoral trigger, twist rotations, etc. --- simgui-ds.json | 8 ++++++-- simgui-window.json | 18 +++++++++--------- src/main/java/frc/robot/RobotStates.java | 1 + src/main/java/frc/robot/intake/Intake.java | 16 ++++++++++++++++ .../java/frc/robot/intake/IntakeStates.java | 2 ++ src/main/java/frc/robot/pilot/PilotStates.java | 2 ++ src/main/java/frc/robot/twist/Twist.java | 5 +++-- src/main/java/frc/robot/twist/TwistStates.java | 2 ++ src/main/java/frc/robot/vision/Vision.java | 8 ++++---- 9 files changed, 45 insertions(+), 17 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index c2e63b37..a3e2ed8e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -106,9 +106,13 @@ } ], "robotJoysticks": [ - {}, { - "guid": "Keyboard1" + "guid": "78696e70757401000000000000000000", + "useGamepad": true + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ], "useEnableDisableHotkeys": true diff --git a/simgui-window.json b/simgui-window.json index ae697db7..aa63a6fb 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -8,11 +8,11 @@ "GLOBAL": { "font": "Proggy Dotted", "fps": "120", - "height": "1009", + "height": "1415", "maximized": "1", "style": "0", "userScale": "2", - "width": "1920", + "width": "2256", "xpos": "0", "ypos": "29" } @@ -34,7 +34,7 @@ "###/SmartDashboard/Auto Chooser": { "Collapsed": "0", "Pos": "321,252", - "Size": "254,54" + "Size": "307,60" }, "###/SmartDashboard/Auto Visualizer": { "Collapsed": "0", @@ -74,17 +74,17 @@ "###Addressable LEDs": { "Collapsed": "0", "Pos": "340,21", - "Size": "330,194" + "Size": "402,220" }, "###FMS": { "Collapsed": "0", "Pos": "746,22", - "Size": "169,184" + "Size": "202,214" }, "###Joysticks": { "Collapsed": "0", "Pos": "271,342", - "Size": "796,121" + "Size": "976,278" }, "###Keyboard 0 Settings": { "Collapsed": "0", @@ -127,12 +127,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "676,639", - "Size": "192,218" + "Size": "232,254" }, "###Timing": { "Collapsed": "0", "Pos": "5,187", - "Size": "135,173" + "Size": "162,194" }, "Debug##Default": { "Collapsed": "0", @@ -142,7 +142,7 @@ "Robot State": { "Collapsed": "0", "Pos": "186,189", - "Size": "99,116" + "Size": "118,134" } } } diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 9b67e848..44ab002a 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -39,6 +39,7 @@ public class RobotStates { public static final SpectrumState rightScore = new SpectrumState("rightScore"); public static final SpectrumState reverse = new SpectrumState("reverse"); public static final SpectrumState actionPrepState = new SpectrumState("actionPrepState"); + public static final SpectrumState algaeAfterAction = new SpectrumState("algaeAfterAction"); public static final SpectrumState actionState = new SpectrumState("actionState"); public static final SpectrumState homeAll = new SpectrumState("homeAll"); public static final SpectrumState autonStationIntake = new SpectrumState("autonStationIntake"); diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 7fc84d88..c709cf21 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -149,6 +149,22 @@ public boolean hasIntakeGamePiece() { && Math.abs(motorCurrent) > config.hasGamePieceCurrent); } + public boolean hasCoral() { + double motorOutput = getVelocityRPM(); + double motorCurrent = getStatorCurrent(); + double motorVoltage = getVoltage(); + return (Math.abs(motorOutput) < config.hasGamePieceVelocity + && Math.abs(motorCurrent) > config.hasGamePieceCurrent) && motorVoltage > 0; + } + + public boolean hasAlgae() { + double motorOutput = getVelocityRPM(); + double motorCurrent = getStatorCurrent(); + double motorVoltage = getVoltage(); + return (Math.abs(motorOutput) < config.hasGamePieceVelocity + && Math.abs(motorCurrent) > config.hasGamePieceCurrent) && motorVoltage < 0; + } + public Command intakeCoral(DoubleSupplier torque, DoubleSupplier current) { return new FunctionalCommand( () -> setCurrentLimits(current, torque), diff --git a/src/main/java/frc/robot/intake/IntakeStates.java b/src/main/java/frc/robot/intake/IntakeStates.java index dbda24dd..4a8ba721 100644 --- a/src/main/java/frc/robot/intake/IntakeStates.java +++ b/src/main/java/frc/robot/intake/IntakeStates.java @@ -20,6 +20,8 @@ public class IntakeStates { .and(photon); public static final Trigger hasGamePiece = new Trigger(intake::hasIntakeGamePiece); + public static final Trigger hasCoral = new Trigger(intake::hasCoral); + public static final Trigger hasAlgae = new Trigger(intake::hasAlgae); public static void setupDefaultCommand() { intake.setDefaultCommand( diff --git a/src/main/java/frc/robot/pilot/PilotStates.java b/src/main/java/frc/robot/pilot/PilotStates.java index 7b5f0efc..077b301e 100644 --- a/src/main/java/frc/robot/pilot/PilotStates.java +++ b/src/main/java/frc/robot/pilot/PilotStates.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Robot; +import frc.robot.intake.IntakeStates; import frc.spectrumLib.Telemetry; /** This class should have any command calls that directly call the Pilot */ @@ -20,6 +21,7 @@ public static void setStates() { pilot.upReorient .or(pilot.downReorient, pilot.leftReorient, pilot.rightReorient) .onTrue(log(rumble(1, 0.5).withName("Pilot.reorientRumble"))); + IntakeStates.hasCoral.onTrue(log(rumble(1, 0.5).withName("Pilot.coralRumble"))); } /** Command that can be used to rumble the pilot controller */ diff --git a/src/main/java/frc/robot/twist/Twist.java b/src/main/java/frc/robot/twist/Twist.java index 04824a89..36d4b36a 100644 --- a/src/main/java/frc/robot/twist/Twist.java +++ b/src/main/java/frc/robot/twist/Twist.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Robot; import frc.robot.RobotSim; +import frc.robot.intake.IntakeStates; import frc.spectrumLib.Rio; import frc.spectrumLib.SpectrumCANcoder; import frc.spectrumLib.Telemetry; @@ -248,8 +249,8 @@ private void setDegrees(DoubleSupplier degrees) { public Command twistHome() { return run( () -> { - if (algae.getAsBoolean()) { - setDegrees(config::getAlgaeIntake); + if (algae.getAsBoolean() || IntakeStates.hasAlgae.getAsBoolean()) { + setDegrees(config::getRightCoral); } else if (coral.getAsBoolean()) { setDegrees(config::getLeftCoral); } else { diff --git a/src/main/java/frc/robot/twist/TwistStates.java b/src/main/java/frc/robot/twist/TwistStates.java index 9829eaf7..a671567a 100644 --- a/src/main/java/frc/robot/twist/TwistStates.java +++ b/src/main/java/frc/robot/twist/TwistStates.java @@ -30,6 +30,8 @@ public static void setStates() { algae.or(groundAlgae, stagedAlgae) .whileTrue(twist.moveToDegrees(config::getAlgaeIntake).withName("Twist.Algae")); + algae.onFalse(twist.moveToDegrees(config::getRightCoral).withName("Twist.HomeAlgae")); + Robot.getPilot() .groundAlgae_RT .whileTrue( diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index b83702d3..d8a50d68 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -366,7 +366,7 @@ public void setLimelightPipelines(int pipeline) { limelight.setLimelightPipeline(pipeline); } } - + // ------------------------------------------------------------------------------ // Calculation Functions // ------------------------------------------------------------------------------ @@ -458,8 +458,6 @@ public double getAdjustedThetaToReefFace() { return -1; // no reef tag found } - - /** Returns the distance from the reef in meters, adjusted for the robot's movement. */ public double[] getDistanceToReefFromRobot() { RawFiducial[] frontTags = frontLL.getRawFiducial(); @@ -505,7 +503,9 @@ public double[] getDistanceToReefFromRobot() { */ public Translation2d getAdjustedReefPos() { - int reefID = (int) frontLL.getClosestTagID(); // must call closestReefFace before this method gets passed + int reefID = + (int) frontLL.getClosestTagID(); // must call closestReefFace before this method + // gets passed Pose2d[] reefFaces = Field.Reef.getCenterFaces(); double NORM_FUDGE = 0.075; // double tunableNoteVelocity = 1; From ee014e1afe2ea1c5c9e82d0c3c67cd92a6a5a9ea Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Tue, 11 Mar 2025 19:47:44 -0500 Subject: [PATCH 02/19] simple trigger for algae and coral --- src/main/java/frc/robot/intake/Intake.java | 16 ---------------- src/main/java/frc/robot/intake/IntakeStates.java | 4 ++-- 2 files changed, 2 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index c709cf21..7fc84d88 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -149,22 +149,6 @@ public boolean hasIntakeGamePiece() { && Math.abs(motorCurrent) > config.hasGamePieceCurrent); } - public boolean hasCoral() { - double motorOutput = getVelocityRPM(); - double motorCurrent = getStatorCurrent(); - double motorVoltage = getVoltage(); - return (Math.abs(motorOutput) < config.hasGamePieceVelocity - && Math.abs(motorCurrent) > config.hasGamePieceCurrent) && motorVoltage > 0; - } - - public boolean hasAlgae() { - double motorOutput = getVelocityRPM(); - double motorCurrent = getStatorCurrent(); - double motorVoltage = getVoltage(); - return (Math.abs(motorOutput) < config.hasGamePieceVelocity - && Math.abs(motorCurrent) > config.hasGamePieceCurrent) && motorVoltage < 0; - } - public Command intakeCoral(DoubleSupplier torque, DoubleSupplier current) { return new FunctionalCommand( () -> setCurrentLimits(current, torque), diff --git a/src/main/java/frc/robot/intake/IntakeStates.java b/src/main/java/frc/robot/intake/IntakeStates.java index 4a8ba721..c843d33a 100644 --- a/src/main/java/frc/robot/intake/IntakeStates.java +++ b/src/main/java/frc/robot/intake/IntakeStates.java @@ -20,8 +20,8 @@ public class IntakeStates { .and(photon); public static final Trigger hasGamePiece = new Trigger(intake::hasIntakeGamePiece); - public static final Trigger hasCoral = new Trigger(intake::hasCoral); - public static final Trigger hasAlgae = new Trigger(intake::hasAlgae); + public static final Trigger hasCoral = hasGamePiece.and(intake.aboveVelocityRPM(() -> 0, () -> 0)); + public static final Trigger hasAlgae = hasGamePiece.and(intake.belowVelocityRPM(() -> 0, () -> 0)); public static void setupDefaultCommand() { intake.setDefaultCommand( From b01b57c0a3d3d07d9a6d5cff38f435d9bba72bf6 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Wed, 12 Mar 2025 15:03:52 -0500 Subject: [PATCH 03/19] remove algae after score + twist correct way --- src/main/java/frc/robot/RobotStates.java | 11 ++++++++++ .../java/frc/robot/intake/IntakeStates.java | 6 +++-- src/main/java/frc/robot/pilot/Pilot.java | 1 + .../java/frc/robot/pilot/PilotStates.java | 2 +- src/main/java/frc/robot/twist/Twist.java | 12 ++++++++++ .../java/frc/robot/twist/TwistStates.java | 8 +++++-- src/main/java/frc/robot/vision/Vision.java | 22 +++++++++++++++++++ 7 files changed, 57 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 44ab002a..7db23a1e 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -13,6 +13,7 @@ import frc.robot.pilot.Pilot; import frc.robot.shoulder.ShoulderStates; import frc.robot.swerve.Swerve; +import frc.robot.vision.Vision; import frc.spectrumLib.Rio; import frc.spectrumLib.SpectrumState; import frc.spectrumLib.util.Util; @@ -176,6 +177,16 @@ public static void setupStates() { algae.setTrue(), coral.setFalse(), l3.setTrue(), actionPrepState.setTrue()); pilot.l3AlgaeRemoval.onFalse(l3.setFalse(), actionPrepState.setFalse()); + if(Robot.getVision().isL3Algae()) { + pilot.algaeRemovalAfterScore.onTrue( + algae.setTrue(), coral.setFalse(), l3.setTrue(), actionPrepState.setTrue()); + pilot.algaeRemovalAfterScore.onFalse(l3.setFalse(), actionPrepState.setFalse()); + } else { + pilot.algaeRemovalAfterScore.onTrue( + algae.setTrue(), coral.setFalse(), l2.setTrue(), actionPrepState.setTrue()); + pilot.algaeRemovalAfterScore.onFalse(l2.setFalse(), actionPrepState.setFalse()); + } + // ********************************** // Staging and Scoring diff --git a/src/main/java/frc/robot/intake/IntakeStates.java b/src/main/java/frc/robot/intake/IntakeStates.java index c843d33a..80ecd91f 100644 --- a/src/main/java/frc/robot/intake/IntakeStates.java +++ b/src/main/java/frc/robot/intake/IntakeStates.java @@ -20,8 +20,10 @@ public class IntakeStates { .and(photon); public static final Trigger hasGamePiece = new Trigger(intake::hasIntakeGamePiece); - public static final Trigger hasCoral = hasGamePiece.and(intake.aboveVelocityRPM(() -> 0, () -> 0)); - public static final Trigger hasAlgae = hasGamePiece.and(intake.belowVelocityRPM(() -> 0, () -> 0)); + public static final Trigger hasCoral = + hasGamePiece.and(intake.aboveVelocityRPM(() -> 0, () -> 0)); + public static final Trigger hasAlgae = + hasGamePiece.and(intake.belowVelocityRPM(() -> 0, () -> 0)); public static void setupDefaultCommand() { intake.setDefaultCommand( diff --git a/src/main/java/frc/robot/pilot/Pilot.java b/src/main/java/frc/robot/pilot/Pilot.java index 480351ea..77e094df 100644 --- a/src/main/java/frc/robot/pilot/Pilot.java +++ b/src/main/java/frc/robot/pilot/Pilot.java @@ -29,6 +29,7 @@ public class Pilot extends Gamepad { public final Trigger l2AlgaeRemoval = X.and(teleop); public final Trigger l3AlgaeRemoval = Y.and(teleop); + public final Trigger algaeRemovalAfterScore = B.and(teleop); public final Trigger photonRemoveL3Algae = groundCoral_LB_RT.and(photon); public final Trigger climbRoutine_start = start.and(noFn, teleop); diff --git a/src/main/java/frc/robot/pilot/PilotStates.java b/src/main/java/frc/robot/pilot/PilotStates.java index 87af93f6..ccdeaefa 100644 --- a/src/main/java/frc/robot/pilot/PilotStates.java +++ b/src/main/java/frc/robot/pilot/PilotStates.java @@ -3,8 +3,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Robot; -import frc.robot.vision.VisionStates; import frc.robot.intake.IntakeStates; +import frc.robot.vision.VisionStates; import frc.spectrumLib.Telemetry; /** This class should have any command calls that directly call the Pilot */ diff --git a/src/main/java/frc/robot/twist/Twist.java b/src/main/java/frc/robot/twist/Twist.java index 36d4b36a..598349ac 100644 --- a/src/main/java/frc/robot/twist/Twist.java +++ b/src/main/java/frc/robot/twist/Twist.java @@ -242,6 +242,18 @@ public Command moveToDegrees(DoubleSupplier degrees) { return super.moveToDegrees(degrees).withName(getName() + ".runPoseDegrees"); } + public Command moveToDegrees(DoubleSupplier degrees, boolean shortestPath) { + if (shortestPath) { + return moveToDegrees(degrees); + } else { + if (degrees.getAsDouble() < 0) { + return moveToDegrees(() -> degrees.getAsDouble() + 360); + } else { + return moveToDegrees(() -> degrees.getAsDouble() - 360); + } + } + } + private void setDegrees(DoubleSupplier degrees) { setMMPositionFoc(() -> degreesToRotations(degrees)); } diff --git a/src/main/java/frc/robot/twist/TwistStates.java b/src/main/java/frc/robot/twist/TwistStates.java index a671567a..74f0cd12 100644 --- a/src/main/java/frc/robot/twist/TwistStates.java +++ b/src/main/java/frc/robot/twist/TwistStates.java @@ -44,9 +44,13 @@ public static void setStates() { netAlgae.whileTrue(twist.moveToDegrees(config::getNet).withName("Twist.Net")); branch.and(rightScore) - .whileTrue(twist.moveToDegrees(config::getRightCoral).withName("Twist.rightCoral")); + .whileTrue( + twist.moveToDegrees(config::getRightCoral, true) + .withName("Twist.rightCoral")); branch.and(rightScore.not()) - .whileTrue(twist.moveToDegrees(config::getLeftCoral).withName("Twist.leftCoral")); + .whileTrue( + twist.moveToDegrees(config::getLeftCoral, false) + .withName("Twist.leftCoral")); } public static Command coastMode() { diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index df80a890..30fbba07 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -649,6 +649,28 @@ public double[] getDistanceToReefFromRobot() { return seenReefFaces; } + public boolean isL3Algae() { + int frontTag = (int) frontLL.getClosestTagID(); + int backTag = (int) backLL.getClosestTagID(); + boolean isL3 = false; + + if(Field.isBlue()) { + if(frontTag == 22 || frontTag == 20 || frontTag == 18 || backTag == 22 || backTag == 20 || backTag == 18) { + isL3 = true; + } else { + isL3 = false; + } + } else { + if(frontTag == 11 || frontTag == 9 || frontTag == 7 || backTag == 11 || backTag == 9 || backTag == 7) { + isL3 = true; + } else { + isL3 = false; + } + } + + return isL3; + } + /** * Gets a field-relative position for the score to the reef the robot should align, adjusted for * the robot's movement. From 4647f5903742b99178e43453e76596be1fd2c854 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Wed, 12 Mar 2025 15:06:45 -0500 Subject: [PATCH 04/19] merge fixes --- src/main/java/frc/robot/RobotStates.java | 3 +- src/main/java/frc/robot/vision/Vision.java | 32 ++++++++++++++++++++++ 2 files changed, 33 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 80c54683..267ec5c7 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -13,7 +13,6 @@ import frc.robot.pilot.Pilot; import frc.robot.shoulder.ShoulderStates; import frc.robot.swerve.Swerve; -import frc.robot.vision.Vision; import frc.spectrumLib.Rio; import frc.spectrumLib.SpectrumState; import frc.spectrumLib.util.Util; @@ -177,7 +176,7 @@ public static void setupStates() { algae.setTrue(), coral.setFalse(), l3.setTrue(), actionPrepState.setTrue()); pilot.l3AlgaeRemoval.onFalse(l3.setFalse(), actionPrepState.setFalse()); - if(Robot.getVision().isL3Algae()) { + if (Robot.getVision().isL3Algae()) { pilot.algaeRemovalAfterScore.onTrue( algae.setTrue(), coral.setFalse(), l3.setTrue(), actionPrepState.setTrue()); pilot.algaeRemovalAfterScore.onFalse(l3.setFalse(), actionPrepState.setFalse()); diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index 2c8cf99d..b96c9ca4 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -578,6 +578,38 @@ public double getTagTX() { } } + public boolean isL3Algae() { + int frontTag = (int) frontLL.getClosestTagID(); + int backTag = (int) backLL.getClosestTagID(); + boolean isL3 = false; + + if (Field.isBlue()) { + if (frontTag == 22 + || frontTag == 20 + || frontTag == 18 + || backTag == 22 + || backTag == 20 + || backTag == 18) { + isL3 = true; + } else { + isL3 = false; + } + } else { + if (frontTag == 11 + || frontTag == 9 + || frontTag == 7 + || backTag == 11 + || backTag == 9 + || backTag == 7) { + isL3 = true; + } else { + isL3 = false; + } + } + + return isL3; + } + // ------------------------------------------------------------------------------ // VisionStates Commands // ------------------------------------------------------------------------------ From 62850e3c0dbe456708940d0049a35849d585ae34 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Wed, 12 Mar 2025 16:14:14 -0500 Subject: [PATCH 05/19] testing changes --- src/main/java/frc/robot/configs/PM2025.java | 2 +- src/main/java/frc/robot/twist/Twist.java | 2 +- src/main/java/frc/robot/twist/TwistStates.java | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/configs/PM2025.java b/src/main/java/frc/robot/configs/PM2025.java index 410564c1..c4285a15 100644 --- a/src/main/java/frc/robot/configs/PM2025.java +++ b/src/main/java/frc/robot/configs/PM2025.java @@ -16,6 +16,6 @@ public PM2025() { elbow.setAttached(true); twist.setAttached(true); intake.setAttached(true); - climb.setAttached(true); + climb.setAttached(false); } } diff --git a/src/main/java/frc/robot/twist/Twist.java b/src/main/java/frc/robot/twist/Twist.java index 598349ac..dd19690d 100644 --- a/src/main/java/frc/robot/twist/Twist.java +++ b/src/main/java/frc/robot/twist/Twist.java @@ -101,7 +101,7 @@ public TwistConfig() { configStatorCurrentLimit(torqueCurrentLimit, true); configForwardTorqueCurrentLimit(torqueCurrentLimit); configReverseTorqueCurrentLimit(torqueCurrentLimit); - configMinMaxRotations(-.25, 0.5); + configMinMaxRotations(-0.25, 0.5); configReverseSoftLimit(getMinRotations(), true); configForwardSoftLimit(getMaxRotations(), true); configNeutralBrakeMode(true); diff --git a/src/main/java/frc/robot/twist/TwistStates.java b/src/main/java/frc/robot/twist/TwistStates.java index 74f0cd12..402fb64e 100644 --- a/src/main/java/frc/robot/twist/TwistStates.java +++ b/src/main/java/frc/robot/twist/TwistStates.java @@ -45,11 +45,11 @@ public static void setStates() { branch.and(rightScore) .whileTrue( - twist.moveToDegrees(config::getRightCoral, true) + twist.moveToDegrees(config::getRightCoral, false) .withName("Twist.rightCoral")); branch.and(rightScore.not()) .whileTrue( - twist.moveToDegrees(config::getLeftCoral, false) + twist.moveToDegrees(config::getLeftCoral, true) .withName("Twist.leftCoral")); } From 9ca4d7af57cadf3a213e101cba8faada1a7dfe3d Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Fri, 14 Mar 2025 13:52:12 -0500 Subject: [PATCH 06/19] testing changes --- src/main/java/frc/robot/RobotStates.java | 7 +++++-- src/main/java/frc/robot/twist/Twist.java | 12 ------------ src/main/java/frc/robot/twist/TwistStates.java | 4 ++-- 3 files changed, 7 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 267ec5c7..bc7bb9d0 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -157,6 +157,9 @@ public static void setupStates() { operator.algaeStage.or(operator.coralStage).onTrue(actionState.setFalse()); + pilot.algaeRemovalAfterScore.onTrue(algaeAfterAction.setTrue()); + actionState.onFalse(algaeAfterAction.setFalse()); + // ********************************* // Intaking States stationIntaking.whileTrue(coral.toggleToTrue(), algae.setFalse()); @@ -177,11 +180,11 @@ public static void setupStates() { pilot.l3AlgaeRemoval.onFalse(l3.setFalse(), actionPrepState.setFalse()); if (Robot.getVision().isL3Algae()) { - pilot.algaeRemovalAfterScore.onTrue( + pilot.algaeRemovalAfterScore.whileTrue( algae.setTrue(), coral.setFalse(), l3.setTrue(), actionPrepState.setTrue()); pilot.algaeRemovalAfterScore.onFalse(l3.setFalse(), actionPrepState.setFalse()); } else { - pilot.algaeRemovalAfterScore.onTrue( + pilot.algaeRemovalAfterScore.whileTrue( algae.setTrue(), coral.setFalse(), l2.setTrue(), actionPrepState.setTrue()); pilot.algaeRemovalAfterScore.onFalse(l2.setFalse(), actionPrepState.setFalse()); } diff --git a/src/main/java/frc/robot/twist/Twist.java b/src/main/java/frc/robot/twist/Twist.java index dd19690d..184f47e3 100644 --- a/src/main/java/frc/robot/twist/Twist.java +++ b/src/main/java/frc/robot/twist/Twist.java @@ -242,18 +242,6 @@ public Command moveToDegrees(DoubleSupplier degrees) { return super.moveToDegrees(degrees).withName(getName() + ".runPoseDegrees"); } - public Command moveToDegrees(DoubleSupplier degrees, boolean shortestPath) { - if (shortestPath) { - return moveToDegrees(degrees); - } else { - if (degrees.getAsDouble() < 0) { - return moveToDegrees(() -> degrees.getAsDouble() + 360); - } else { - return moveToDegrees(() -> degrees.getAsDouble() - 360); - } - } - } - private void setDegrees(DoubleSupplier degrees) { setMMPositionFoc(() -> degreesToRotations(degrees)); } diff --git a/src/main/java/frc/robot/twist/TwistStates.java b/src/main/java/frc/robot/twist/TwistStates.java index 402fb64e..00d90634 100644 --- a/src/main/java/frc/robot/twist/TwistStates.java +++ b/src/main/java/frc/robot/twist/TwistStates.java @@ -45,11 +45,11 @@ public static void setStates() { branch.and(rightScore) .whileTrue( - twist.moveToDegrees(config::getRightCoral, false) + twist.moveToDegrees(config::getRightCoral) .withName("Twist.rightCoral")); branch.and(rightScore.not()) .whileTrue( - twist.moveToDegrees(config::getLeftCoral, true) + twist.moveToDegrees(config::getLeftCoral) .withName("Twist.leftCoral")); } From 9e95170b06d022b0c399a91a5d1d514ea742be4d Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Fri, 14 Mar 2025 14:50:28 -0500 Subject: [PATCH 07/19] testing fixes --- src/main/java/frc/robot/RobotStates.java | 13 ++++++++++--- src/main/java/frc/robot/twist/TwistStates.java | 8 ++------ src/main/java/frc/spectrumLib/SpectrumState.java | 7 +++++++ 3 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index bc7bb9d0..effa76ff 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -150,7 +150,9 @@ public static void setupStates() { .or(autonActionOn) .onTrue(actionPrepState.setTrue(), actionState.setFalse()); - actionPrepState.or(autonActionOn).onTrue(actionState.setFalse()); + actionPrepState + .or(autonActionOn) + .onTrue(actionState.setFalse(), algaeAfterAction.setFalse()); actionPrepState.onChangeToFalse(actionState.setTrueForTime(RobotStates::getScoreTime)); autonActionOff.onChangeToFalse(actionState.setTrueForTime(RobotStates::getScoreTime)); @@ -158,7 +160,11 @@ public static void setupStates() { operator.algaeStage.or(operator.coralStage).onTrue(actionState.setFalse()); pilot.algaeRemovalAfterScore.onTrue(algaeAfterAction.setTrue()); - actionState.onFalse(algaeAfterAction.setFalse()); + actionState + .and(algaeAfterAction) + .onTrue( + l2.setTrueAfterTime(RobotStates::getScoreTime), + algae.setTrueAfterTime(RobotStates::getScoreTime)); // ********************************* // Intaking States @@ -251,7 +257,8 @@ public static Command clearStaged() { rightScore.setFalse(), coral.setFalse(), algae.setFalse(), - extendedState.setFalse()) + extendedState.setFalse(), + algaeAfterAction.setFalse()) .withName("Clear Staged"); } diff --git a/src/main/java/frc/robot/twist/TwistStates.java b/src/main/java/frc/robot/twist/TwistStates.java index 00d90634..a671567a 100644 --- a/src/main/java/frc/robot/twist/TwistStates.java +++ b/src/main/java/frc/robot/twist/TwistStates.java @@ -44,13 +44,9 @@ public static void setStates() { netAlgae.whileTrue(twist.moveToDegrees(config::getNet).withName("Twist.Net")); branch.and(rightScore) - .whileTrue( - twist.moveToDegrees(config::getRightCoral) - .withName("Twist.rightCoral")); + .whileTrue(twist.moveToDegrees(config::getRightCoral).withName("Twist.rightCoral")); branch.and(rightScore.not()) - .whileTrue( - twist.moveToDegrees(config::getLeftCoral) - .withName("Twist.leftCoral")); + .whileTrue(twist.moveToDegrees(config::getLeftCoral).withName("Twist.leftCoral")); } public static Command coastMode() { diff --git a/src/main/java/frc/spectrumLib/SpectrumState.java b/src/main/java/frc/spectrumLib/SpectrumState.java index 8c9373db..4ab10010 100644 --- a/src/main/java/frc/spectrumLib/SpectrumState.java +++ b/src/main/java/frc/spectrumLib/SpectrumState.java @@ -72,6 +72,13 @@ public Command setTrueForTime(DoubleSupplier time) { .withName(name + " state: SetTrueForTime->" + time.getAsDouble()); } + public Command setTrueAfterTime(DoubleSupplier time) { + return new WaitCommand(time.getAsDouble()) + .andThen(setTrue()) + .ignoringDisable(true) + .withName(name + " state: SetTrueAfterTime->" + time.getAsDouble()); + } + /** * Command to set state to false, and then to true, ensuring your state will trigger actions * From 17a7f729bbd1c1b8bcd88fd0dca01dd6bedcd916 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Fri, 14 Mar 2025 15:27:04 -0500 Subject: [PATCH 08/19] more testing --- src/main/java/frc/robot/RobotStates.java | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index effa76ff..1a7ae142 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -160,11 +160,6 @@ public static void setupStates() { operator.algaeStage.or(operator.coralStage).onTrue(actionState.setFalse()); pilot.algaeRemovalAfterScore.onTrue(algaeAfterAction.setTrue()); - actionState - .and(algaeAfterAction) - .onTrue( - l2.setTrueAfterTime(RobotStates::getScoreTime), - algae.setTrueAfterTime(RobotStates::getScoreTime)); // ********************************* // Intaking States @@ -186,13 +181,17 @@ public static void setupStates() { pilot.l3AlgaeRemoval.onFalse(l3.setFalse(), actionPrepState.setFalse()); if (Robot.getVision().isL3Algae()) { - pilot.algaeRemovalAfterScore.whileTrue( - algae.setTrue(), coral.setFalse(), l3.setTrue(), actionPrepState.setTrue()); - pilot.algaeRemovalAfterScore.onFalse(l3.setFalse(), actionPrepState.setFalse()); + actionState + .and(algaeAfterAction) + .onTrue( + l3.setTrueAfterTime(RobotStates::getScoreTime), + algae.setTrueAfterTime(RobotStates::getScoreTime)); } else { - pilot.algaeRemovalAfterScore.whileTrue( - algae.setTrue(), coral.setFalse(), l2.setTrue(), actionPrepState.setTrue()); - pilot.algaeRemovalAfterScore.onFalse(l2.setFalse(), actionPrepState.setFalse()); + actionState + .and(algaeAfterAction) + .onTrue( + l2.setTrueAfterTime(RobotStates::getScoreTime), + algae.setTrueAfterTime(RobotStates::getScoreTime)); } // ********************************** From 1cb2c15ae7862eb5c3c8c9ec34a3472915df18d0 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Fri, 14 Mar 2025 15:52:52 -0500 Subject: [PATCH 09/19] more changes --- src/main/java/frc/robot/RobotStates.java | 16 +++++----------- src/main/java/frc/robot/twist/Twist.java | 3 +-- 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 1a7ae142..c5f7ff32 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -150,9 +150,7 @@ public static void setupStates() { .or(autonActionOn) .onTrue(actionPrepState.setTrue(), actionState.setFalse()); - actionPrepState - .or(autonActionOn) - .onTrue(actionState.setFalse(), algaeAfterAction.setFalse()); + actionPrepState.or(autonActionOn).onTrue(actionState.setFalse()); actionPrepState.onChangeToFalse(actionState.setTrueForTime(RobotStates::getScoreTime)); autonActionOff.onChangeToFalse(actionState.setTrueForTime(RobotStates::getScoreTime)); @@ -182,16 +180,12 @@ public static void setupStates() { if (Robot.getVision().isL3Algae()) { actionState - .and(algaeAfterAction) - .onTrue( - l3.setTrueAfterTime(RobotStates::getScoreTime), - algae.setTrueAfterTime(RobotStates::getScoreTime)); + .and(algaeAfterAction) + .onTrue(l3.setTrue(), algae.setTrue()); } else { actionState - .and(algaeAfterAction) - .onTrue( - l2.setTrueAfterTime(RobotStates::getScoreTime), - algae.setTrueAfterTime(RobotStates::getScoreTime)); + .and(algaeAfterAction) + .onTrue(l2.setTrue(), algae.setTrue()); } // ********************************** diff --git a/src/main/java/frc/robot/twist/Twist.java b/src/main/java/frc/robot/twist/Twist.java index 184f47e3..10ad1eb7 100644 --- a/src/main/java/frc/robot/twist/Twist.java +++ b/src/main/java/frc/robot/twist/Twist.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Robot; import frc.robot.RobotSim; -import frc.robot.intake.IntakeStates; import frc.spectrumLib.Rio; import frc.spectrumLib.SpectrumCANcoder; import frc.spectrumLib.Telemetry; @@ -249,7 +248,7 @@ private void setDegrees(DoubleSupplier degrees) { public Command twistHome() { return run( () -> { - if (algae.getAsBoolean() || IntakeStates.hasAlgae.getAsBoolean()) { + if (algae.getAsBoolean()) { setDegrees(config::getRightCoral); } else if (coral.getAsBoolean()) { setDegrees(config::getLeftCoral); From d28a6114c6bedbc6cd838687b2307055d1abec8d Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Fri, 14 Mar 2025 16:37:28 -0500 Subject: [PATCH 10/19] test changes --- src/main/java/frc/robot/RobotStates.java | 16 ++++++++++++++-- src/main/java/frc/spectrumLib/SpectrumState.java | 7 +++++++ 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index cbd36d1e..7cd4afb7 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -150,11 +150,23 @@ public static void setupStates() { if (Robot.getVision().isL3Algae()) { actionState .and(algaeAfterAction) - .onTrue(l3.setTrue(), algae.setTrue()); + .onTrue( + l3.setTrueAfterTime(RobotStates::getScoreTime), + algae.setTrue(), + coral.setFalse(), + l1.setFalseAfterTime(RobotStates::getScoreTime), + l2.setFalseAfterTime(RobotStates::getScoreTime), + l4.setFalseAfterTime(RobotStates::getScoreTime)); } else { actionState .and(algaeAfterAction) - .onTrue(l2.setTrue(), algae.setTrue()); + .onTrue( + l2.setTrueAfterTime(RobotStates::getScoreTime), + algae.setTrue(), + coral.setFalse(), + l1.setFalseAfterTime(RobotStates::getScoreTime), + l3.setFalseAfterTime(RobotStates::getScoreTime), + l4.setFalseAfterTime(RobotStates::getScoreTime)); } // ********************************** diff --git a/src/main/java/frc/spectrumLib/SpectrumState.java b/src/main/java/frc/spectrumLib/SpectrumState.java index 4ab10010..d1112f9d 100644 --- a/src/main/java/frc/spectrumLib/SpectrumState.java +++ b/src/main/java/frc/spectrumLib/SpectrumState.java @@ -79,6 +79,13 @@ public Command setTrueAfterTime(DoubleSupplier time) { .withName(name + " state: SetTrueAfterTime->" + time.getAsDouble()); } + public Command setFalseAfterTime(DoubleSupplier time) { + return new WaitCommand(time.getAsDouble()) + .andThen(setFalse()) + .ignoringDisable(true) + .withName(name + " state: SetTrueAfterTime->" + time.getAsDouble()); + } + /** * Command to set state to false, and then to true, ensuring your state will trigger actions * From b4bd6b681fc1c1a9fdb9302f63d8ab962a891fe6 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Fri, 14 Mar 2025 18:29:08 -0500 Subject: [PATCH 11/19] final edit --- src/main/java/frc/robot/RobotStates.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 7cd4afb7..fa663c85 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -152,7 +152,7 @@ public static void setupStates() { .and(algaeAfterAction) .onTrue( l3.setTrueAfterTime(RobotStates::getScoreTime), - algae.setTrue(), + algae.setTrueAfterTime(() -> (RobotStates.getScoreTime()/2)), coral.setFalse(), l1.setFalseAfterTime(RobotStates::getScoreTime), l2.setFalseAfterTime(RobotStates::getScoreTime), @@ -162,7 +162,7 @@ public static void setupStates() { .and(algaeAfterAction) .onTrue( l2.setTrueAfterTime(RobotStates::getScoreTime), - algae.setTrue(), + algae.setTrueAfterTime(() -> (RobotStates.getScoreTime()/2)), coral.setFalse(), l1.setFalseAfterTime(RobotStates::getScoreTime), l3.setFalseAfterTime(RobotStates::getScoreTime), From 91e33894e49e6cc120d6075925d29a5718e270b1 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Tue, 18 Mar 2025 16:58:17 -0500 Subject: [PATCH 12/19] robotstate changes --- simgui-ds.json | 3 ++- simgui-window.json | 18 ++++++++-------- src/main/java/frc/robot/RobotStates.java | 27 ++++++++++++++---------- 3 files changed, 27 insertions(+), 21 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 44bd084c..a3e2ed8e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -107,7 +107,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true }, { "guid": "78696e70757401000000000000000000", diff --git a/simgui-window.json b/simgui-window.json index de462d0d..947d4a36 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -8,11 +8,11 @@ "GLOBAL": { "font": "Proggy Dotted", "fps": "120", - "height": "2036", + "height": "1415", "maximized": "1", "style": "0", "userScale": "2", - "width": "1920", + "width": "2256", "xpos": "0", "ypos": "29" } @@ -34,7 +34,7 @@ "###/SmartDashboard/Auto Chooser": { "Collapsed": "0", "Pos": "321,252", - "Size": "254,54" + "Size": "307,60" }, "###/SmartDashboard/Auto Visualizer": { "Collapsed": "0", @@ -74,17 +74,17 @@ "###Addressable LEDs": { "Collapsed": "0", "Pos": "340,21", - "Size": "330,194" + "Size": "402,220" }, "###FMS": { "Collapsed": "0", "Pos": "746,22", - "Size": "169,184" + "Size": "202,214" }, "###Joysticks": { "Collapsed": "0", "Pos": "271,342", - "Size": "796,121" + "Size": "976,278" }, "###Keyboard 0 Settings": { "Collapsed": "0", @@ -127,12 +127,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "676,639", - "Size": "192,218" + "Size": "232,254" }, "###Timing": { "Collapsed": "0", "Pos": "5,187", - "Size": "135,173" + "Size": "162,194" }, "Debug##Default": { "Collapsed": "0", @@ -142,7 +142,7 @@ "Robot State": { "Collapsed": "0", "Pos": "186,189", - "Size": "99,116" + "Size": "118,134" } } } diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index fa663c85..b820a81f 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -100,9 +100,12 @@ public static void setupStates() { pilot.home_select.or(operator.home_select).onFalse(clearStates()); autonClearStates.whileTrue(clearStates()); - actionState - .or(operator.staged) - .onChangeToFalse(coral.setFalse().alongWith(algae.setFalse())); + actionState.or(operator.staged).onChangeToFalse(coral.setFalse()); + algaeAfterAction + .not() + .and(actionState.or(operator.staged)) + .onChangeToFalse(algae.setFalse()); + algae.and(actionState).onTrue(algaeAfterAction.setFalse()); isAtHome.onTrue(homeAll.setFalse()); pilot.coastOn_dB.or(operator.coastOn_dB).onTrue(coastMode.setTrue().ignoringDisable(true)); @@ -151,22 +154,24 @@ public static void setupStates() { actionState .and(algaeAfterAction) .onTrue( - l3.setTrueAfterTime(RobotStates::getScoreTime), - algae.setTrueAfterTime(() -> (RobotStates.getScoreTime()/2)), - coral.setFalse(), + l3.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 1)), + algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 1)), + coral.setFalseAfterTime(() -> (RobotStates.getScoreTime() / 2)), l1.setFalseAfterTime(RobotStates::getScoreTime), l2.setFalseAfterTime(RobotStates::getScoreTime), - l4.setFalseAfterTime(RobotStates::getScoreTime)); + l4.setFalseAfterTime(RobotStates::getScoreTime), + homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2)); } else { actionState .and(algaeAfterAction) .onTrue( - l2.setTrueAfterTime(RobotStates::getScoreTime), - algae.setTrueAfterTime(() -> (RobotStates.getScoreTime()/2)), - coral.setFalse(), + l2.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 1)), + algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 1)), + coral.setFalseAfterTime(() -> (RobotStates.getScoreTime() / 2)), l1.setFalseAfterTime(RobotStates::getScoreTime), l3.setFalseAfterTime(RobotStates::getScoreTime), - l4.setFalseAfterTime(RobotStates::getScoreTime)); + l4.setFalseAfterTime(RobotStates::getScoreTime), + homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2)); } // ********************************** From 163adc6b01dae92bbb72ee3d948c09b78b04a210 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Tue, 18 Mar 2025 17:29:31 -0500 Subject: [PATCH 13/19] merge conflict --- src/main/java/frc/robot/pilot/Pilot.java | 1 + src/main/java/frc/robot/twist/TwistStates.java | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/pilot/Pilot.java b/src/main/java/frc/robot/pilot/Pilot.java index e0907307..8fd16e68 100644 --- a/src/main/java/frc/robot/pilot/Pilot.java +++ b/src/main/java/frc/robot/pilot/Pilot.java @@ -28,6 +28,7 @@ public class Pilot extends Gamepad { public final Trigger l2AlgaeRemoval = X.and(teleop); public final Trigger l3AlgaeRemoval = Y.and(teleop); public final Trigger photonRemoveL3Algae = rightTrigger.and(fn, teleop, photon); + public final Trigger algaeRemovalAfterScore = B.and(teleop); public final Trigger climbRoutine_start = start.and(noFn, teleop); diff --git a/src/main/java/frc/robot/twist/TwistStates.java b/src/main/java/frc/robot/twist/TwistStates.java index 19e8345b..84afb0dc 100644 --- a/src/main/java/frc/robot/twist/TwistStates.java +++ b/src/main/java/frc/robot/twist/TwistStates.java @@ -29,8 +29,6 @@ public static void setStates() { stagedAlgae.whileTrue(move(config::getAlgaeIntake, "Twist.Algae")); - algae.onFalse(twist.moveToDegrees(config::getRightCoral).withName("Twist.HomeAlgae")); - Robot.getPilot() .groundAlgae_RT .whileTrue(move(config::getGroundAlgaeIntake, "Twist.AlgaeIntake")); From 876f88702215d1ee77c25df219abf56426f5509f Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Tue, 18 Mar 2025 18:25:47 -0500 Subject: [PATCH 14/19] vision update --- src/main/java/frc/robot/RobotStates.java | 47 ++++++++++++------------ src/main/java/frc/robot/twist/Twist.java | 2 +- 2 files changed, 24 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index c0c96285..79ecd459 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -22,7 +22,7 @@ public class RobotStates { private static final Pilot pilot = Robot.getPilot(); private static final Operator operator = Robot.getOperator(); - @Getter private static double scoreTime = 3.0; + @Getter private static double scoreTime = 2.0; // Robot States // These are states that aren't directly tied to hardware or buttons, etc. @@ -86,6 +86,8 @@ public class RobotStates { public static final Trigger isAtHome = ElevatorStates.isHome.and(ElbowStates.isHome, ShoulderStates.isHome); + public static final Trigger isL3Algae = new Trigger(() -> Robot.getVision().isL3Algae()); + // reset triggers public static final Trigger homeElevator = operator.homeElevator_A; @@ -151,29 +153,26 @@ public static void setupStates() { algae.setTrue(), coral.setFalse(), l3.setTrue(), actionPrepState.setTrue()); pilot.l3AlgaeRemoval.onFalse(l3.setFalse(), actionPrepState.setFalse()); - if (Robot.getVision().isL3Algae()) { - actionState - .and(algaeAfterAction) - .onTrue( - l3.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 1)), - algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 1)), - coral.setFalseAfterTime(() -> (RobotStates.getScoreTime() / 2)), - l1.setFalseAfterTime(RobotStates::getScoreTime), - l2.setFalseAfterTime(RobotStates::getScoreTime), - l4.setFalseAfterTime(RobotStates::getScoreTime), - homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2)); - } else { - actionState - .and(algaeAfterAction) - .onTrue( - l2.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 1)), - algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 1)), - coral.setFalseAfterTime(() -> (RobotStates.getScoreTime() / 2)), - l1.setFalseAfterTime(RobotStates::getScoreTime), - l3.setFalseAfterTime(RobotStates::getScoreTime), - l4.setFalseAfterTime(RobotStates::getScoreTime), - homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2)); - } + actionState + .and(algaeAfterAction, L3Algae) + .onTrue( + l3.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), + algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), + coral.setFalseAfterTime(() -> (RobotStates.getScoreTime() / 2)), + l1.setFalseAfterTime(RobotStates::getScoreTime), + l2.setFalseAfterTime(RobotStates::getScoreTime), + l4.setFalseAfterTime(RobotStates::getScoreTime), + homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2)); + actionState + .and(algaeAfterAction, L3Algae.not()) + .onTrue( + l2.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), + algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), + coral.setFalseAfterTime(() -> (RobotStates.getScoreTime() / 2)), + l1.setFalseAfterTime(RobotStates::getScoreTime), + l3.setFalseAfterTime(RobotStates::getScoreTime), + l4.setFalseAfterTime(RobotStates::getScoreTime), + homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2)); // ********************************** // Staging and Scoring diff --git a/src/main/java/frc/robot/twist/Twist.java b/src/main/java/frc/robot/twist/Twist.java index 507fa266..8d87a078 100644 --- a/src/main/java/frc/robot/twist/Twist.java +++ b/src/main/java/frc/robot/twist/Twist.java @@ -308,7 +308,7 @@ public Command twistHome() { return run( () -> { if (algae.getAsBoolean()) { - setDegrees(config::getRightCoral); + setDegrees(config::getAlgaeIntake); } else if (coral.getAsBoolean()) { setDegrees(config::getLeftCoral); } else { From 656547d4f75e0b01667e5c882ae02788f193b18b Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Tue, 25 Mar 2025 16:15:26 -0500 Subject: [PATCH 15/19] small test changes --- simgui-ds.json | 6 ++---- simgui.json | 5 ++++- src/main/java/frc/robot/RobotStates.java | 6 ++++-- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index a3e2ed8e..30247095 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -107,12 +107,10 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" }, { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard1" } ], "useEnableDisableHotkeys": true diff --git a/simgui.json b/simgui.json index f2da99fb..74e10b74 100644 --- a/simgui.json +++ b/simgui.json @@ -58,7 +58,10 @@ }, "right": 3466, "top": 291, - "width": 17.54825210571289 + "width": 17.54825210571289, + "window": { + "visible": true + } }, "/SmartDashboard/Field2d": { "bottom": 1476, diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 0525ebbb..ee2be7a5 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -166,20 +166,22 @@ public static void setupStates() { pilot.l3AlgaeRemoval.onFalse(l3.setFalse(), actionPrepState.setFalse()); actionState - .and(algaeAfterAction, L3Algae) + .and(algaeAfterAction, isL3Algae) .onTrue( l3.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), + actionPrepState.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), coral.setFalseAfterTime(() -> (RobotStates.getScoreTime() / 2)), l1.setFalseAfterTime(RobotStates::getScoreTime), l2.setFalseAfterTime(RobotStates::getScoreTime), l4.setFalseAfterTime(RobotStates::getScoreTime), homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2)); actionState - .and(algaeAfterAction, L3Algae.not()) + .and(algaeAfterAction, isL3Algae.not()) .onTrue( l2.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), + actionPrepState.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), coral.setFalseAfterTime(() -> (RobotStates.getScoreTime() / 2)), l1.setFalseAfterTime(RobotStates::getScoreTime), l3.setFalseAfterTime(RobotStates::getScoreTime), From 3bed4e77f2895ebede339d403f611c31c5dbd75b Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Wed, 26 Mar 2025 18:20:30 -0500 Subject: [PATCH 16/19] photon vision comits --- simgui-window.json | 17 +++++--- simgui.json | 38 ++++++++++++----- src/main/java/frc/robot/RobotStates.java | 6 +++ src/main/java/frc/robot/vision/Vision.java | 21 ++++------ .../java/frc/robot/vision/VisionSystem.java | 42 +++++++++++-------- 5 files changed, 76 insertions(+), 48 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index d193a1b0..a0bde3ba 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -1,23 +1,28 @@ { "Docking": { "Data": [ - "DockNode ID=0x00000001 Pos=7,990 Size=905,316 Selected=0x48913727" + "DockNode ID=0x00000001 Pos=7,990 Size=905,316 Selected=0x2D9B9756" ] }, "MainWindow": { "GLOBAL": { "font": "Proggy Dotted", "fps": "120", - "height": "1415", + "height": "1405", "maximized": "1", "style": "0", "userScale": "2", - "width": "2256", + "width": "1126", "xpos": "0", "ypos": "29" } }, "Table": { + "0x965B9C9E,2": { + "Column 0 Width": "160", + "Column 1 Weight": "1.0000", + "RefScale": "16" + }, "0xE56EC1C2,4": { "Column 0 Weight": "1.0000", "Column 1 Weight": "1.0000", @@ -43,12 +48,12 @@ }, "###/SmartDashboard/Field2d": { "Collapsed": "0", - "Pos": "8,639", + "Pos": "11,639", "Size": "650,348" }, "###/SmartDashboard/LeftView": { "Collapsed": "0", - "Pos": "1675,640", + "Pos": "1107,648", "Size": "544,547" }, "###/SmartDashboard/Scheduler": { @@ -73,7 +78,7 @@ }, "###/SmartDashboard/frontView": { "Collapsed": "0", - "Pos": "1299,640", + "Pos": "1107,648", "Size": "373,555" }, "###Addressable LEDs": { diff --git a/simgui.json b/simgui.json index 74e10b74..27db3970 100644 --- a/simgui.json +++ b/simgui.json @@ -58,18 +58,24 @@ }, "right": 3466, "top": 291, - "width": 17.54825210571289, - "window": { - "visible": true - } + "width": 17.54825210571289 }, "/SmartDashboard/Field2d": { - "bottom": 1476, - "height": 8.210550308227539, - "left": 150, - "right": 2961, - "top": 79, - "width": 16.541748046875, + "bottom": 1638, + "builtin": "2025 Reefscape", + "height": 8.051901817321777, + "left": 534, + "limelight-back": { + "arrows": false, + "style": "Hidden" + }, + "limelight-front": { + "arrows": false, + "style": "Hidden" + }, + "right": 3466, + "top": 291, + "width": 17.54825210571289, "window": { "visible": true } @@ -137,6 +143,15 @@ "NetworkTables": { "transitory": { "CameraPublisher": { + "limelight-back-processed": { + "open": true + }, + "limelight-back-raw": { + "open": true + }, + "limelight-front-processed": { + "open": true + }, "open": true }, "Robot": { @@ -160,6 +175,9 @@ } }, "NetworkTables Info": { + "Server": { + "open": true + }, "visible": true } } diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index ee2be7a5..dd802ae1 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -44,6 +44,8 @@ public class RobotStates { public static final SpectrumState homeAll = new SpectrumState("homeAll"); public static final SpectrumState autonStationIntake = new SpectrumState("autonStationIntake"); + public static final SpectrumState testL3 = new SpectrumState("testL3"); + /** * 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 @@ -99,6 +101,7 @@ public class RobotStates { ElevatorStates.isHome.and(ElbowStates.isHome, ShoulderStates.isHome); public static final Trigger isL3Algae = new Trigger(() -> Robot.getVision().isL3Algae()); + public static final Trigger seesTag = new Trigger(() -> Robot.getVision().seesTag()); // reset triggers public static final Trigger homeElevator = operator.homeElevator_A; @@ -165,6 +168,9 @@ public static void setupStates() { algae.setTrue(), coral.setFalse(), l3.setTrue(), actionPrepState.setTrue()); pilot.l3AlgaeRemoval.onFalse(l3.setFalse(), actionPrepState.setFalse()); + seesTag.onTrue(testL3.setTrue()); + seesTag.onFalse(testL3.setFalse()); + actionState .and(algaeAfterAction, isL3Algae) .onTrue( diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index cd6aac55..5c188598 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -752,28 +752,17 @@ public static double indexOfSmallest(double[] array) { // } public boolean isL3Algae() { - int frontTag = (int) frontLL.getClosestTagID(); - int backTag = (int) backLL.getClosestTagID(); + int tag = (int) getClosestTagID(); boolean isL3 = false; if (Field.isBlue()) { - if (frontTag == 22 - || frontTag == 20 - || frontTag == 18 - || backTag == 22 - || backTag == 20 - || backTag == 18) { + if (tag == 22 || tag == 20 || tag == 18) { isL3 = true; } else { isL3 = false; } } else { - if (frontTag == 11 - || frontTag == 9 - || frontTag == 7 - || backTag == 11 - || backTag == 9 - || backTag == 7) { + if (tag == 11 || tag == 9 || tag == 7) { isL3 = true; } else { isL3 = false; @@ -783,6 +772,10 @@ public boolean isL3Algae() { return isL3; } + public boolean seesTag() { + return frontLL.targetInView() || backLL.targetInView(); + } + // ------------------------------------------------------------------------------ // VisionStates Commands // ------------------------------------------------------------------------------ diff --git a/src/main/java/frc/robot/vision/VisionSystem.java b/src/main/java/frc/robot/vision/VisionSystem.java index 776a47ac..87af2e34 100644 --- a/src/main/java/frc/robot/vision/VisionSystem.java +++ b/src/main/java/frc/robot/vision/VisionSystem.java @@ -10,26 +10,19 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.io.IOException; import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; public class VisionSystem extends SubsystemBase { @SuppressWarnings("unused") private final PhotonCamera camera = new PhotonCamera("cameraName"); - // private final PhotonCamera frontCam = new PhotonCamera(VisionConfig.FRONT_LL); - // private final PhotonCamera backCam = new PhotonCamera(VisionConfig.RIGHT_LL); + + private final PhotonCamera frontCam = new PhotonCamera("limelight-front"); + private final PhotonCamera backCam = new PhotonCamera("limelight-back"); private final VisionSystemSim visionSim = new VisionSystemSim("main"); private final Pose2dSupplier getSimPose; - Transform3d robotToFrontCamera = - new Transform3d( - new Translation3d(0, 0, 0.5), // Centered on the robot, 0.5m up - new Rotation3d(0, Math.toRadians(-15), 0)); // Pitched 15 deg up - Transform3d robotToBackCamera = - new Transform3d( - new Translation3d(0, 0, 0.5), // Centered on the robot, 0.5m up - new Rotation3d(0, Math.toRadians(-15), 0)); // Pitched 15 deg up - @FunctionalInterface public interface Pose2dSupplier { Pose2d getPose2d(); @@ -46,15 +39,20 @@ public VisionSystem(Pose2dSupplier getSimPose) { props.setLatencyStdDevMs(5.0); // Setup simulated camera - // PhotonCameraSim cameraSimFront = new PhotonCameraSim(frontCam, props); - // PhotonCameraSim cameraSimBack = new PhotonCameraSim(backCam, props); - // Draw field wireframe in simulated camera view - // cameraSimFront.enableDrawWireframe(true); - // cameraSimBack.enableDrawWireframe(false); + PhotonCameraSim cameraSimFront = new PhotonCameraSim(frontCam, props); + Translation3d robotToFrontCameraTrl = new Translation3d(0.215, 0, 0.188); + Rotation3d robotToFrontCameraRot = new Rotation3d(0, Math.toRadians(0), 0); + Transform3d robotToFrontCamera = + new Transform3d(robotToFrontCameraTrl, robotToFrontCameraRot); + + PhotonCameraSim cameraSimBack = new PhotonCameraSim(backCam, props); + Translation3d robotToBackCameraTrl = new Translation3d(-0.215, 0.0, 0.188); + Rotation3d robotToBackCameraRot = new Rotation3d(0, Math.toRadians(0), Math.toRadians(180)); + Transform3d robotToBackCamera = new Transform3d(robotToBackCameraTrl, robotToBackCameraRot); // // Add simulated camera to vision sim - // visionSim.addCamera(cameraSimFront, robotToFrontCamera); - // visionSim.addCamera(cameraSimBack, robotToBackCamera); + visionSim.addCamera(cameraSimFront, robotToFrontCamera); + visionSim.addCamera(cameraSimBack, robotToBackCamera); // Add AprilTags to vision sim try { @@ -65,6 +63,14 @@ public VisionSystem(Pose2dSupplier getSimPose) { } catch (IOException e) { System.err.println(e); } + + visionSim.getDebugField(); + cameraSimFront.enableRawStream(true); + cameraSimFront.enableProcessedStream(true); + + // Draw field wireframe in simulated camera view + cameraSimFront.enableDrawWireframe(true); + cameraSimBack.enableDrawWireframe(false); } @Override From 36865650505cf1688f40efb71bcb3f1ac2848614 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Wed, 26 Mar 2025 20:05:49 -0500 Subject: [PATCH 17/19] timing speeds --- src/main/java/frc/robot/RobotStates.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index dd802ae1..a7c5c5ef 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -23,7 +23,7 @@ public class RobotStates { private static final Pilot pilot = Robot.getPilot(); private static final Operator operator = Robot.getOperator(); - @Getter private static double scoreTime = 2.0; + @Getter private static double scoreTime = 1.5; // Robot States // These are states that aren't directly tied to hardware or buttons, etc. @@ -181,7 +181,7 @@ public static void setupStates() { l1.setFalseAfterTime(RobotStates::getScoreTime), l2.setFalseAfterTime(RobotStates::getScoreTime), l4.setFalseAfterTime(RobotStates::getScoreTime), - homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2)); + homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2 + 0.2)); actionState .and(algaeAfterAction, isL3Algae.not()) .onTrue( @@ -192,7 +192,7 @@ public static void setupStates() { l1.setFalseAfterTime(RobotStates::getScoreTime), l3.setFalseAfterTime(RobotStates::getScoreTime), l4.setFalseAfterTime(RobotStates::getScoreTime), - homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2)); + homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2 + 0.2)); // ********************************** // Staging and Scoring From 6ec63afe956391b9b677b683c61d0c80b563f33d Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Wed, 26 Mar 2025 20:25:53 -0500 Subject: [PATCH 18/19] twist clockwise --- src/main/java/frc/robot/twist/TwistStates.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/twist/TwistStates.java b/src/main/java/frc/robot/twist/TwistStates.java index 0c902453..a3158587 100644 --- a/src/main/java/frc/robot/twist/TwistStates.java +++ b/src/main/java/frc/robot/twist/TwistStates.java @@ -44,7 +44,7 @@ public static void setStates() { branch.and((rightScore.not().or(Robot.getOperator().leftScore)), actionPrepState) .whileTrue(move(config::getLeftCoral, config::getStageDelay, "Twist.leftCoral")); - twistL4R.onTrue(move(config::getRightCoral, "Twist.RightCoral")); + twistL4R.onTrue(move(config::getRightCoral, false, "Twist.RightCoral")); twistL4L.onTrue(move(config::getLeftCoral, "Twist.leftCoral")); climbPrep.whileTrue(move(config::getClimbPrep, "Twist.climbPrep")); @@ -54,6 +54,10 @@ public static Command move(DoubleSupplier degrees, String name) { return twist.move(degrees).withName(name); } + public static Command move(DoubleSupplier degrees, Boolean clockwise, String name) { + return twist.move(degrees, clockwise).withName(name); + } + public static Command move(DoubleSupplier degrees, DoubleSupplier delay, String name) { return new WaitCommand(delay.getAsDouble()).andThen(move(degrees, name).withName(name)); } From d868bc676282faa296ea86f716fd1886c034ba02 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Thu, 27 Mar 2025 17:06:18 -0500 Subject: [PATCH 19/19] less magic numbers --- simgui-window.json | 10 +++++----- simgui.json | 5 ++++- src/main/java/frc/robot/RobotStates.java | 11 ++++++----- 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index a0bde3ba..ca0df641 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -1,18 +1,18 @@ { "Docking": { "Data": [ - "DockNode ID=0x00000001 Pos=7,990 Size=905,316 Selected=0x2D9B9756" + "DockNode ID=0x00000001 Pos=7,990 Size=905,316 Selected=0x48913727" ] }, "MainWindow": { "GLOBAL": { "font": "Proggy Dotted", "fps": "120", - "height": "1405", + "height": "1415", "maximized": "1", "style": "0", "userScale": "2", - "width": "1126", + "width": "2256", "xpos": "0", "ypos": "29" } @@ -53,7 +53,7 @@ }, "###/SmartDashboard/LeftView": { "Collapsed": "0", - "Pos": "1107,648", + "Pos": "1684,663", "Size": "544,547" }, "###/SmartDashboard/Scheduler": { @@ -78,7 +78,7 @@ }, "###/SmartDashboard/frontView": { "Collapsed": "0", - "Pos": "1107,648", + "Pos": "1297,660", "Size": "373,555" }, "###Addressable LEDs": { diff --git a/simgui.json b/simgui.json index 27db3970..e453c9b8 100644 --- a/simgui.json +++ b/simgui.json @@ -58,7 +58,10 @@ }, "right": 3466, "top": 291, - "width": 17.54825210571289 + "width": 17.54825210571289, + "window": { + "visible": true + } }, "/SmartDashboard/Field2d": { "bottom": 1638, diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index a7c5c5ef..dd651bb6 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/src/main/java/frc/robot/RobotStates.java @@ -23,7 +23,8 @@ public class RobotStates { private static final Pilot pilot = Robot.getPilot(); private static final Operator operator = Robot.getOperator(); - @Getter private static double scoreTime = 1.5; + @Getter private static double scoreTime = 1.0; + @Getter private static double scoreTimeAlgae = 0.5; // Robot States // These are states that aren't directly tied to hardware or buttons, etc. @@ -177,22 +178,22 @@ public static void setupStates() { l3.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), actionPrepState.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), - coral.setFalseAfterTime(() -> (RobotStates.getScoreTime() / 2)), + coral.setFalseAfterTime(() -> (RobotStates.getScoreTimeAlgae())), l1.setFalseAfterTime(RobotStates::getScoreTime), l2.setFalseAfterTime(RobotStates::getScoreTime), l4.setFalseAfterTime(RobotStates::getScoreTime), - homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2 + 0.2)); + homeAll.setTrueAfterTime(() -> RobotStates.getScoreTimeAlgae() + 0.2)); actionState .and(algaeAfterAction, isL3Algae.not()) .onTrue( l2.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), actionPrepState.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)), - coral.setFalseAfterTime(() -> (RobotStates.getScoreTime() / 2)), + coral.setFalseAfterTime(() -> (RobotStates.getScoreTimeAlgae())), l1.setFalseAfterTime(RobotStates::getScoreTime), l3.setFalseAfterTime(RobotStates::getScoreTime), l4.setFalseAfterTime(RobotStates::getScoreTime), - homeAll.setTrueAfterTime(() -> RobotStates.getScoreTime() / 2 + 0.2)); + homeAll.setTrueAfterTime(() -> RobotStates.getScoreTimeAlgae() + 0.2)); // ********************************** // Staging and Scoring