From 6f8492afc637bb655ed2f93252925e164aa5f096 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Sun, 19 Jan 2025 20:05:52 -0500 Subject: [PATCH 01/21] yuh --- .../main/java/choreo/auto/AutoRoutine.java | 60 +++++++++++- .../main/java/choreo/auto/AutoTrajectory.java | 93 ++++++++++++++++++- .../src/test/java/choreo/auto/DoneTest.java | 22 ++++- src/styles.css | 6 +- 4 files changed, 170 insertions(+), 11 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index fa3abeedea..33942dbc97 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -43,6 +43,9 @@ public class AutoRoutine { /** A boolean utilized in {@link #active()} to resolve trueness */ private boolean isActive = false; + /** A boolean indicating if a trajectory is running on the routine right now */ + private boolean isIdle = true; + /** A boolean that is true when the loop is killed */ boolean isKilled = false; @@ -114,6 +117,15 @@ int pollCount() { return pollCount; } + /** + * Updates the idle state of the routine. + * + * @param isIdle The new idle state of the routine. + */ + void updateIdle(boolean isIdle) { + this.isIdle = isIdle; + } + /** * Resets the routine. This can either be called on auto init or auto end to reset the routine * incase you run it again. If this is called on a routine that doesn't need to be reset it will @@ -135,6 +147,17 @@ public void kill() { isKilled = true; } + /** + * Creates a trigger that is true when the routine is idle. + * + *

Idle is defined as no trajectories made by the routine are running. + * + * @return A trigger that is true when the routine is idle. + */ + public Trigger idle() { + return new Trigger(loop, () -> isIdle); + } + /** * Creates a new {@link AutoTrajectory} to be used in an auto routine. * @@ -188,16 +211,32 @@ public Trigger anyDone(AutoTrajectory trajectory, AutoTrajectory... trajectories * @param trajectory The first trajectory to watch. * @param trajectories The other trajectories to watch * @return a trigger that determines if any of the trajectories are finished + * @see AutoTrajectory#doneDelay(int) */ - public Trigger anyDone( + public Trigger anyDoneDelay( int cyclesToDelay, AutoTrajectory trajectory, AutoTrajectory... trajectories) { - var trigger = trajectory.done(cyclesToDelay); + var trigger = trajectory.doneDelay(cyclesToDelay); for (int i = 0; i < trajectories.length; i++) { - trigger = trigger.or(trajectories[i].done(cyclesToDelay)); + trigger = trigger.or(trajectories[i].doneDelay(cyclesToDelay)); } return trigger.and(this.active()); } + /** + * Creates a trigger that produces a rising edge when any of the trajectories are finished. + * + * @param cyclesToDelay The number of cycles to delay. + * @param trajectory The first trajectory to watch. + * @param trajectories The other trajectories to watch + * @return a trigger that determines if any of the trajectories are finished + * @see AutoTrajectory#doneDelay(int) + * @see AutoTrajectory#anyDoneDelay(int) + */ + public Trigger anyDone( + int cyclesToDelay, AutoTrajectory trajectory, AutoTrajectory... trajectories) { + return anyDoneDelay(cyclesToDelay, trajectory, trajectories); + } + /** * Creates a trigger that returns true when any of the trajectories given are active. * @@ -213,6 +252,21 @@ public Trigger anyActive(AutoTrajectory trajectory, AutoTrajectory... trajectori return trigger.and(this.active()); } + /** + * Creates a trigger that returns true when any of the trajectories given are inactive. + * + * @param trajectory The first trajectory to watch. + * @param trajectories The other trajectories to watch + * @return a trigger that determines if any of the trajectories are inactive + */ + public Trigger anyInactive(AutoTrajectory trajectory, AutoTrajectory... trajectories) { + var trigger = trajectory.inactive(); + for (int i = 0; i < trajectories.length; i++) { + trigger = trigger.or(trajectories[i].inactive()); + } + return trigger.and(this.active()); + } + /** * Creates a command that will poll this event loop and reset it when it is cancelled. * diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index b1b64063eb..d072704166 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.Trigger; import java.util.Optional; @@ -145,6 +146,7 @@ private void cmdInitialize() { isActive = true; isCompleted = false; logTrajectory(true); + routine.updateIdle(false); } @SuppressWarnings("unchecked") @@ -173,6 +175,7 @@ private void cmdEnd(boolean interrupted) { isCompleted = !interrupted; cmdExecute(); // will force the controller to be fed the final sample logTrajectory(false); + routine.updateIdle(true); } private boolean cmdIsFinished() { @@ -199,6 +202,18 @@ public Command cmd() { .withName("Trajectory_" + name); } + /** + * Creates a command that will schedule another command that will follow the trajectory. + * + *

This can be useful when putting {@link AutoTrajectory} commands in sequences that require + * subsystems that the AutoTrajectory triggers require. + * + * @return The command that will schedule the trajectory following command. + */ + public Command spawnCmd() { + return new ScheduleCommand(cmd()).withName("Trajectory_" + name + "_Spawner"); + } + /** * Creates a command that resets the robot's odometry to the start of this trajectory. * @@ -293,7 +308,21 @@ public Trigger inactive() { } /** - * Returns a trigger that rises to true when the trajectory ends and falls after one pulse. + * Returns a trigger that rises to true a number of cycles after the trajectory ends and falls + * after one pulse. + * + * @param cyclesToDelay The number of cycles to delay the trigger from rising to true. + * @return A trigger that is true when the trajectory is finished. + * @see #doneDelay(int) + */ + @Deprecated(forRemoval = true, since = "2025") + public Trigger done(int cyclesToDelay) { + return done(cyclesToDelay); + } + + /** + * Returns a trigger that rises to true a number of cycles after the trajectory ends and falls + * after one pulse. * *

This is different from inactive() in a few ways. * @@ -316,8 +345,8 @@ public Trigger inactive() { * * routine.enabled().onTrue(rushMidTraj.cmd()); * - * rushMidTraj.done(10).and(noGamepiece).onTrue(pickupAnotherGamepiece.cmd()); - * rushMidTraj.done(10).and(hasGamepiece).onTrue(goShootGamepiece.cmd()); + * rushMidTraj.doneDelay(10).and(noGamepiece).onTrue(pickupAnotherGamepiece.cmd()); + * rushMidTraj.doneDelay(10).and(hasGamepiece).onTrue(goShootGamepiece.cmd()); * * // If done never falls when a new trajectory is scheduled * // then these triggers leak into the next trajectory, causing the next note pickup @@ -327,7 +356,7 @@ public Trigger inactive() { * @param cyclesToDelay The number of cycles to delay the trigger from rising to true. * @return A trigger that is true when the trajectory is finished. */ - public Trigger done(int cyclesToDelay) { + public Trigger doneDelay(int cyclesToDelay) { BooleanSupplier checker = new BooleanSupplier() { /** The last used value for trajectory completeness */ @@ -394,6 +423,62 @@ public Trigger done() { return done(0); } + public Trigger doneFor(int cycles) { + BooleanSupplier enterPulse = doneDelay(0); + BooleanSupplier exitPulse = doneDelay(cycles + 1); + BooleanSupplier checker = + new BooleanSupplier() { + boolean output = false; + + @Override + public boolean getAsBoolean() { + if (enterPulse.getAsBoolean()) { + output = true; + } + if (exitPulse.getAsBoolean()) { + output = false; + } + return output; + } + }; + return inactive().and(new Trigger(routine.loop(), checker)); + } + + /** + * Returns a trigger that is true when the trajectory was the last one active and is done. + * + * @return A trigger that is true when the trajectory was the last one active and is done. + */ + public Trigger recentlyDone() { + BooleanSupplier enterPulse = doneDelay(0); + BooleanSupplier exitPulse = routine.idle().negate(); + BooleanSupplier checker = + new BooleanSupplier() { + boolean output = false; + + @Override + public boolean getAsBoolean() { + if (enterPulse.getAsBoolean()) { + output = true; + } + if (exitPulse.getAsBoolean()) { + output = false; + } + return output; + } + }; + return inactive().and(new Trigger(routine.loop(), checker)); + } + + /** + * A shorthand for `.done().onTrue(otherTrajectory.cmd())` + * + * @param otherTrajectory The other trajectory to run when this one is done. + */ + public void chain(AutoTrajectory otherTrajectory) { + done().onTrue(otherTrajectory.cmd()); + } + /** * Returns a trigger that will go true for 1 cycle when the desired time has elapsed * diff --git a/choreolib/src/test/java/choreo/auto/DoneTest.java b/choreolib/src/test/java/choreo/auto/DoneTest.java index 7eddac8456..901595f2a1 100644 --- a/choreolib/src/test/java/choreo/auto/DoneTest.java +++ b/choreolib/src/test/java/choreo/auto/DoneTest.java @@ -40,7 +40,9 @@ public void testExecution() { AutoTrajectory traj = factory.trajectory(trajectory, routine); BooleanSupplier done = traj.done(); - BooleanSupplier doneDelayed = traj.done(2); + BooleanSupplier doneDelayed = traj.doneDelay(2); + BooleanSupplier doneFor = traj.doneFor(2); + BooleanSupplier recentlyDone = traj.recentlyDone(); SimHooks.pauseTiming(); @@ -58,6 +60,11 @@ public void testExecution() { assertTrue(routine.active().getAsBoolean()); assertTrue(traj.active()); + assertFalse(done); + assertFalse(doneDelayed); + assertFalse(doneFor); + assertFalse(recentlyDone); + SimHooks.stepTiming(1.0); scheduler.run(); SimHooks.stepTiming(1.0); @@ -69,13 +76,26 @@ public void testExecution() { assertTrue(done); assertTrue(done); assertFalse(doneDelayed); + assertTrue(doneFor); + assertTrue(recentlyDone); + scheduler.run(); assertFalse(done); assertFalse(doneDelayed); + assertTrue(doneFor); + assertTrue(recentlyDone); + scheduler.run(); assertFalse(done); assertTrue(doneDelayed); assertTrue(doneDelayed); + assertTrue(doneFor); + assertTrue(recentlyDone); + + routine.updateIdle(false); // simulating to starting a new trajectory + scheduler.run(); + assertFalse(doneFor); + assertFalse(recentlyDone); SimHooks.resumeTiming(); } diff --git a/src/styles.css b/src/styles.css index f5f17d7e1b..4a1df4db71 100644 --- a/src/styles.css +++ b/src/styles.css @@ -1,8 +1,8 @@ body { margin: 0; - font-family: -apple-system, BlinkMacSystemFont, "Segoe UI", "Roboto", - "Oxygen", "Ubuntu", "Cantarell", "Fira Sans", "Droid Sans", - "Helvetica Neue", sans-serif; + font-family: -apple-system, BlinkMacSystemFont, "Segoe UI", "Roboto", "Oxygen", + "Ubuntu", "Cantarell", "Fira Sans", "Droid Sans", "Helvetica Neue", + sans-serif; -webkit-font-smoothing: antialiased; -moz-osx-font-smoothing: grayscale; } From 30b8b9f8ae8075c44072f141a1c9f78fdabbd29a Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Sun, 19 Jan 2025 20:19:07 -0500 Subject: [PATCH 02/21] updated docs --- docs/choreolib/auto-factory.md | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/docs/choreolib/auto-factory.md b/docs/choreolib/auto-factory.md index c177ba5edb..7d93e655ca 100644 --- a/docs/choreolib/auto-factory.md +++ b/docs/choreolib/auto-factory.md @@ -99,6 +99,36 @@ routine.active().onTrue(Commands.print("Started the routine!")); Trajectories can be loaded using `AutoRoutine.trajectory()`, which will return an `AutoTrajectory` ([Java](/api/choreolib/java/choreo/auto/AutoTrajectory.html)). The `AutoTrajectory` class exposes multiple triggers for you to attach reactive logic to, as well as `AutoTrajectory.cmd()` for scheduling the trajectory. +```java +public AutoRoutine pickupAndScoreAuto() { + AutoRoutine routine = autoFactory.newRoutine("taxi"); + + // Load the routine's trajectories + AutoTrajectory driveToMiddle = routine.trajectory("driveToMiddle"); + + // When the routine begins, reset odometry and start the first trajectory (1) + routine.active().onTrue( + Commands.sequence( + driveToMiddle.resetOdometry(), + driveToMiddle.cmd() + ) + ); + + return routine; +} +``` + +AuoTrajectories have a variety of triggers that can be used to attach logic to the trajectory, these include: + +- `active()` - Triggered when the trajectory is active +- `inactive()` - Triggered when the trajectory is inactive, equivalent to `active().negate()` +- `atTime(String)` / `atTime(double)` - Triggered when the trajectory reaches a specific time based on a value or event marker +- `atPose(String, double, double)` / `atPose(Pose2d, double, double)` - Triggered when the robot reaches a specific pose based on a value or event marker +- `done()` - Triggered for 1 cycle after the trajectory is finished +- `doneDelay(int)` - Triggered for 1 cycle `n` cycles after the trajectory is finished +- `doneFor(int)` - Triggered for `n` cycles after the trajectory is finished +- `recentlyDone()` - Triggered after the trajectory is finished until another trajectory is started + ```java public AutoRoutine pickupAndScoreAuto() { AutoRoutine routine = autoFactory.newRoutine("pickupAndScore"); From 9ea3907460bbc86d2d425639c3f5dfe311de366e Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Sun, 19 Jan 2025 20:43:31 -0500 Subject: [PATCH 03/21] fixed javadoc --- .../main/java/choreo/auto/AutoRoutine.java | 16 +++++----- .../main/java/choreo/auto/AutoTrajectory.java | 31 ++++++++++++------- .../src/test/java/choreo/auto/DoneTest.java | 2 +- docs/choreolib/auto-factory.md | 2 +- 4 files changed, 30 insertions(+), 21 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index 33942dbc97..3704a45fbd 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -211,13 +211,13 @@ public Trigger anyDone(AutoTrajectory trajectory, AutoTrajectory... trajectories * @param trajectory The first trajectory to watch. * @param trajectories The other trajectories to watch * @return a trigger that determines if any of the trajectories are finished - * @see AutoTrajectory#doneDelay(int) + * @see AutoTrajectory#doneDelayed(int) */ - public Trigger anyDoneDelay( + public Trigger anyDoneDelayed( int cyclesToDelay, AutoTrajectory trajectory, AutoTrajectory... trajectories) { - var trigger = trajectory.doneDelay(cyclesToDelay); + var trigger = trajectory.doneDelayed(cyclesToDelay); for (int i = 0; i < trajectories.length; i++) { - trigger = trigger.or(trajectories[i].doneDelay(cyclesToDelay)); + trigger = trigger.or(trajectories[i].doneDelayed(cyclesToDelay)); } return trigger.and(this.active()); } @@ -229,12 +229,14 @@ public Trigger anyDoneDelay( * @param trajectory The first trajectory to watch. * @param trajectories The other trajectories to watch * @return a trigger that determines if any of the trajectories are finished - * @see AutoTrajectory#doneDelay(int) - * @see AutoTrajectory#anyDoneDelay(int) + * @see AutoTrajectory#doneDelayed(int) + * @see AutoRoutine#anyDoneDelayed(int) + * @deprecated This method is deprecated and will be removed in 2025. Use {@link #anyDoneDelayed} */ + @Deprecated(forRemoval = true, since = "2025") public Trigger anyDone( int cyclesToDelay, AutoTrajectory trajectory, AutoTrajectory... trajectories) { - return anyDoneDelay(cyclesToDelay, trajectory, trajectories); + return anyDoneDelayed(cyclesToDelay, trajectory, trajectories); } /** diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index d072704166..bd5c8d5dec 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -311,13 +311,14 @@ public Trigger inactive() { * Returns a trigger that rises to true a number of cycles after the trajectory ends and falls * after one pulse. * - * @param cyclesToDelay The number of cycles to delay the trigger from rising to true. + * @param cycles The number of cycles to delay the trigger from rising to true. * @return A trigger that is true when the trajectory is finished. - * @see #doneDelay(int) + * @see #doneDelayed(int) + * @deprecated Use {@link #doneDelayed(int)} instead. */ @Deprecated(forRemoval = true, since = "2025") - public Trigger done(int cyclesToDelay) { - return done(cyclesToDelay); + public Trigger done(int cycles) { + return done(cycles); } /** @@ -345,18 +346,18 @@ public Trigger done(int cyclesToDelay) { * * routine.enabled().onTrue(rushMidTraj.cmd()); * - * rushMidTraj.doneDelay(10).and(noGamepiece).onTrue(pickupAnotherGamepiece.cmd()); - * rushMidTraj.doneDelay(10).and(hasGamepiece).onTrue(goShootGamepiece.cmd()); + * rushMidTraj.doneDelayed(10).and(noGamepiece).onTrue(pickupAnotherGamepiece.cmd()); + * rushMidTraj.doneDelayed(10).and(hasGamepiece).onTrue(goShootGamepiece.cmd()); * * // If done never falls when a new trajectory is scheduled * // then these triggers leak into the next trajectory, causing the next note pickup * // to trigger goShootGamepiece.cmd() even if we no longer care about these checks * * - * @param cyclesToDelay The number of cycles to delay the trigger from rising to true. + * @param cycles The number of cycles to delay the trigger from rising to true. * @return A trigger that is true when the trajectory is finished. */ - public Trigger doneDelay(int cyclesToDelay) { + public Trigger doneDelayed(int cycles) { BooleanSupplier checker = new BooleanSupplier() { /** The last used value for trajectory completeness */ @@ -376,7 +377,7 @@ public boolean getAsBoolean() { // if just flipped to completed update last seen value // and store the cycleTarget based of the current cycle lastCompleteness = true; - cycleTarget = routine.pollCount() + cyclesToDelay; + cycleTarget = routine.pollCount() + cycles; } // finally if check the cycle matches the target return routine.pollCount() == cycleTarget; @@ -423,9 +424,15 @@ public Trigger done() { return done(0); } + /** + * Returns a trigger that stays true for a number of cycles after the trajectory ends. + * + * @param cycles The number of cycles to stay true after the trajectory ends. + * @return A trigger that stays true for a number of cycles after the trajectory ends. + */ public Trigger doneFor(int cycles) { - BooleanSupplier enterPulse = doneDelay(0); - BooleanSupplier exitPulse = doneDelay(cycles + 1); + BooleanSupplier enterPulse = doneDelayed(0); + BooleanSupplier exitPulse = doneDelayed(cycles + 1); BooleanSupplier checker = new BooleanSupplier() { boolean output = false; @@ -450,7 +457,7 @@ public boolean getAsBoolean() { * @return A trigger that is true when the trajectory was the last one active and is done. */ public Trigger recentlyDone() { - BooleanSupplier enterPulse = doneDelay(0); + BooleanSupplier enterPulse = doneDelayed(0); BooleanSupplier exitPulse = routine.idle().negate(); BooleanSupplier checker = new BooleanSupplier() { diff --git a/choreolib/src/test/java/choreo/auto/DoneTest.java b/choreolib/src/test/java/choreo/auto/DoneTest.java index 901595f2a1..03237a568d 100644 --- a/choreolib/src/test/java/choreo/auto/DoneTest.java +++ b/choreolib/src/test/java/choreo/auto/DoneTest.java @@ -40,7 +40,7 @@ public void testExecution() { AutoTrajectory traj = factory.trajectory(trajectory, routine); BooleanSupplier done = traj.done(); - BooleanSupplier doneDelayed = traj.doneDelay(2); + BooleanSupplier doneDelayed = traj.doneDelayed(2); BooleanSupplier doneFor = traj.doneFor(2); BooleanSupplier recentlyDone = traj.recentlyDone(); diff --git a/docs/choreolib/auto-factory.md b/docs/choreolib/auto-factory.md index 7d93e655ca..5a798fd6a1 100644 --- a/docs/choreolib/auto-factory.md +++ b/docs/choreolib/auto-factory.md @@ -125,7 +125,7 @@ AuoTrajectories have a variety of triggers that can be used to attach logic to t - `atTime(String)` / `atTime(double)` - Triggered when the trajectory reaches a specific time based on a value or event marker - `atPose(String, double, double)` / `atPose(Pose2d, double, double)` - Triggered when the robot reaches a specific pose based on a value or event marker - `done()` - Triggered for 1 cycle after the trajectory is finished -- `doneDelay(int)` - Triggered for 1 cycle `n` cycles after the trajectory is finished +- `doneDelayed(int)` - Triggered for 1 cycle `n` cycles after the trajectory is finished - `doneFor(int)` - Triggered for `n` cycles after the trajectory is finished - `recentlyDone()` - Triggered after the trajectory is finished until another trajectory is started From cc26c10586a7078586b32d88d29d96d671dde57b Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Sun, 19 Jan 2025 20:47:28 -0500 Subject: [PATCH 04/21] i hate javadocs --- choreolib/src/main/java/choreo/auto/AutoRoutine.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index 3704a45fbd..0a9b1db6b3 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -230,7 +230,7 @@ public Trigger anyDoneDelayed( * @param trajectories The other trajectories to watch * @return a trigger that determines if any of the trajectories are finished * @see AutoTrajectory#doneDelayed(int) - * @see AutoRoutine#anyDoneDelayed(int) + * @see AutoRoutine#anyDoneDelayed * @deprecated This method is deprecated and will be removed in 2025. Use {@link #anyDoneDelayed} */ @Deprecated(forRemoval = true, since = "2025") From 1c9387c51594b2a1cfc4edddc9b464506ae383bd Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Sun, 19 Jan 2025 20:54:20 -0500 Subject: [PATCH 05/21] ran fmt --- choreolib/src/main/java/choreo/auto/AutoTrajectory.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index bd5c8d5dec..aa30618b90 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -426,7 +426,7 @@ public Trigger done() { /** * Returns a trigger that stays true for a number of cycles after the trajectory ends. - * + * * @param cycles The number of cycles to stay true after the trajectory ends. * @return A trigger that stays true for a number of cycles after the trajectory ends. */ From a8a0d6acb75394421c1a1caade425e6f9b0543f5 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Sun, 19 Jan 2025 21:19:02 -0500 Subject: [PATCH 06/21] fixed recursion error --- choreolib/src/main/java/choreo/auto/AutoTrajectory.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index aa30618b90..6627c8b325 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -318,7 +318,7 @@ public Trigger inactive() { */ @Deprecated(forRemoval = true, since = "2025") public Trigger done(int cycles) { - return done(cycles); + return doneDelayed(cycles); } /** @@ -421,7 +421,7 @@ public boolean getAsBoolean() { * @return A trigger that is true when the trajectory is finished. */ public Trigger done() { - return done(0); + return doneDelayed(0); } /** From f617d8dc53f26035e00cc98cca95589654e56142 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Sun, 19 Jan 2025 21:20:50 -0500 Subject: [PATCH 07/21] dude what --- src/styles.css | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/styles.css b/src/styles.css index 4a1df4db71..f5f17d7e1b 100644 --- a/src/styles.css +++ b/src/styles.css @@ -1,8 +1,8 @@ body { margin: 0; - font-family: -apple-system, BlinkMacSystemFont, "Segoe UI", "Roboto", "Oxygen", - "Ubuntu", "Cantarell", "Fira Sans", "Droid Sans", "Helvetica Neue", - sans-serif; + font-family: -apple-system, BlinkMacSystemFont, "Segoe UI", "Roboto", + "Oxygen", "Ubuntu", "Cantarell", "Fira Sans", "Droid Sans", + "Helvetica Neue", sans-serif; -webkit-font-smoothing: antialiased; -moz-osx-font-smoothing: grayscale; } From f5fc487756ecace5563a1acd636ced9bac321807 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps <55303619+oh-yes-0-fps@users.noreply.github.com> Date: Mon, 20 Jan 2025 20:50:37 -0500 Subject: [PATCH 08/21] Update choreolib/src/main/java/choreo/auto/AutoTrajectory.java Co-authored-by: shueja <32416547+shueja@users.noreply.github.com> --- choreolib/src/main/java/choreo/auto/AutoTrajectory.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index 6627c8b325..70cf7fc1d8 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -206,7 +206,7 @@ public Command cmd() { * Creates a command that will schedule another command that will follow the trajectory. * *

This can be useful when putting {@link AutoTrajectory} commands in sequences that require - * subsystems that the AutoTrajectory triggers require. + * subsystems also required by in AutoTrajectory-bound subsystems. * * @return The command that will schedule the trajectory following command. */ From 116661a01559459cd64ed67e54ecca43dbbeeea3 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps <55303619+oh-yes-0-fps@users.noreply.github.com> Date: Tue, 21 Jan 2025 15:05:49 -0500 Subject: [PATCH 09/21] Update choreolib/src/main/java/choreo/auto/AutoRoutine.java Co-authored-by: shueja <32416547+shueja@users.noreply.github.com> --- choreolib/src/main/java/choreo/auto/AutoRoutine.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index 0a9b1db6b3..2b24d4ed97 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -210,7 +210,7 @@ public Trigger anyDone(AutoTrajectory trajectory, AutoTrajectory... trajectories * @param cyclesToDelay The number of cycles to delay. * @param trajectory The first trajectory to watch. * @param trajectories The other trajectories to watch - * @return a trigger that determines if any of the trajectories are finished + * @return a trigger that goes true for one cycle whenever any of the trajectories finishes, delayed by the given number of cycles. * @see AutoTrajectory#doneDelayed(int) */ public Trigger anyDoneDelayed( From 29498919126c15b068eea9115b70ef2303d67f51 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Tue, 21 Jan 2025 19:02:08 -0500 Subject: [PATCH 10/21] ran fmt --- choreolib/src/main/java/choreo/auto/AutoRoutine.java | 3 ++- choreolib/src/main/java/choreo/auto/AutoTrajectory.java | 8 ++++---- src/styles.css | 6 +++--- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index c2ee97f5e0..45246e0622 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -210,7 +210,8 @@ public Trigger anyDone(AutoTrajectory trajectory, AutoTrajectory... trajectories * @param cyclesToDelay The number of cycles to delay. * @param trajectory The first trajectory to watch. * @param trajectories The other trajectories to watch - * @return a trigger that goes true for one cycle whenever any of the trajectories finishes, delayed by the given number of cycles. + * @return a trigger that goes true for one cycle whenever any of the trajectories finishes, + * delayed by the given number of cycles. * @see AutoTrajectory#doneDelayed(int) */ public Trigger anyDoneDelayed( diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index d08555e177..801698a9b6 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -448,10 +448,10 @@ public boolean getAsBoolean() { if (exitPulse.getAsBoolean()) { output = false; } - return output; + return output && isCompleted; } }; - return inactive().and(new Trigger(routine.loop(), checker)); + return new Trigger(routine.loop(), checker); } /** @@ -474,10 +474,10 @@ public boolean getAsBoolean() { if (exitPulse.getAsBoolean()) { output = false; } - return output; + return output && isCompleted; } }; - return inactive().and(new Trigger(routine.loop(), checker)); + return new Trigger(routine.loop(), checker); } /** diff --git a/src/styles.css b/src/styles.css index f5f17d7e1b..4a1df4db71 100644 --- a/src/styles.css +++ b/src/styles.css @@ -1,8 +1,8 @@ body { margin: 0; - font-family: -apple-system, BlinkMacSystemFont, "Segoe UI", "Roboto", - "Oxygen", "Ubuntu", "Cantarell", "Fira Sans", "Droid Sans", - "Helvetica Neue", sans-serif; + font-family: -apple-system, BlinkMacSystemFont, "Segoe UI", "Roboto", "Oxygen", + "Ubuntu", "Cantarell", "Fira Sans", "Droid Sans", "Helvetica Neue", + sans-serif; -webkit-font-smoothing: antialiased; -moz-osx-font-smoothing: grayscale; } From 706eb4c14d700d677ec98c67a95570633f8fd73e Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Tue, 21 Jan 2025 19:06:06 -0500 Subject: [PATCH 11/21] fixed deprecation error --- choreolib/src/test/java/choreo/auto/DoneTest.java | 2 +- src/styles.css | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/choreolib/src/test/java/choreo/auto/DoneTest.java b/choreolib/src/test/java/choreo/auto/DoneTest.java index f33ed18e33..61fc111b3f 100644 --- a/choreolib/src/test/java/choreo/auto/DoneTest.java +++ b/choreolib/src/test/java/choreo/auto/DoneTest.java @@ -41,7 +41,7 @@ public void testExecution() { AutoTrajectory traj = factory.trajectory(trajectory, routine, true); Trigger done = traj.done(); - Trigger doneDelayed = traj.done(2); + Trigger doneDelayed = traj.doneDelayed(2); Trigger doneFor = traj.doneFor(2); Trigger recentlyDone = traj.recentlyDone(); diff --git a/src/styles.css b/src/styles.css index 4a1df4db71..49638f43ab 100644 --- a/src/styles.css +++ b/src/styles.css @@ -10,4 +10,4 @@ body { code { font-family: source-code-pro, Menlo, Monaco, Consolas, "Courier New", monospace; -} +} \ No newline at end of file From 2c65b269c67134f02fa01bc3b14cb26a7ad53ddf Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Tue, 21 Jan 2025 19:19:53 -0500 Subject: [PATCH 12/21] i hate formatting --- src/styles.css | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/styles.css b/src/styles.css index 49638f43ab..f5f17d7e1b 100644 --- a/src/styles.css +++ b/src/styles.css @@ -1,8 +1,8 @@ body { margin: 0; - font-family: -apple-system, BlinkMacSystemFont, "Segoe UI", "Roboto", "Oxygen", - "Ubuntu", "Cantarell", "Fira Sans", "Droid Sans", "Helvetica Neue", - sans-serif; + font-family: -apple-system, BlinkMacSystemFont, "Segoe UI", "Roboto", + "Oxygen", "Ubuntu", "Cantarell", "Fira Sans", "Droid Sans", + "Helvetica Neue", sans-serif; -webkit-font-smoothing: antialiased; -moz-osx-font-smoothing: grayscale; } @@ -10,4 +10,4 @@ body { code { font-family: source-code-pro, Menlo, Monaco, Consolas, "Courier New", monospace; -} \ No newline at end of file +} From e14e0722188d6eb6abc6428e8a37342a77332de8 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Tue, 21 Jan 2025 19:44:51 -0500 Subject: [PATCH 13/21] change anyInactive to allInactive --- choreolib/src/main/java/choreo/auto/AutoRoutine.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index 45246e0622..d3f265c74c 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -257,15 +257,17 @@ public Trigger anyActive(AutoTrajectory trajectory, AutoTrajectory... trajectori /** * Creates a trigger that returns true when any of the trajectories given are inactive. + * + *

This trigger will only return true if the routine is active. * * @param trajectory The first trajectory to watch. * @param trajectories The other trajectories to watch * @return a trigger that determines if any of the trajectories are inactive */ - public Trigger anyInactive(AutoTrajectory trajectory, AutoTrajectory... trajectories) { + public Trigger allInactive(AutoTrajectory trajectory, AutoTrajectory... trajectories) { var trigger = trajectory.inactive(); for (int i = 0; i < trajectories.length; i++) { - trigger = trigger.or(trajectories[i].inactive()); + trigger = trigger.and(trajectories[i].inactive()); } return trigger.and(this.active()); } From 094a5985239e82f74cc4c59253f0b8de22b468d4 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Wed, 22 Jan 2025 23:02:24 -0500 Subject: [PATCH 14/21] this should work, having weird test failure issues --- .../main/java/choreo/auto/AutoRoutine.java | 13 +- .../main/java/choreo/auto/AutoTrajectory.java | 186 ++++++++---------- .../src/test/java/choreo/auto/DoneTest.java | 35 +++- 3 files changed, 125 insertions(+), 109 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index d3f265c74c..be5975dac5 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -9,6 +9,7 @@ import choreo.trajectory.TrajectorySample; import choreo.util.ChoreoAlert; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -51,6 +52,8 @@ public class AutoRoutine { /** The amount of times the routine has been polled */ private int pollCount = 0; + /** The timestamp of the current cycle */ + private double cycleTimestamp = 0; /** * Creates a new loop with a specific name and a custom alliance supplier. @@ -85,6 +88,7 @@ public void poll() { return; } pollCount++; + cycleTimestamp = Timer.getFPGATimestamp(); loop.poll(); isActive = true; } @@ -108,14 +112,12 @@ public Trigger observe(BooleanSupplier condition) { return new Trigger(loop, condition); } - /** - * Gets the poll count of the routine. - * - * @return The poll count of the routine. - */ int pollCount() { return pollCount; } + double cycleTimestamp() { + return cycleTimestamp; + } /** * Updates the idle state of the routine. @@ -133,6 +135,7 @@ void updateIdle(boolean isIdle) { */ public void reset() { pollCount = 0; + cycleTimestamp = 0; isActive = false; } diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index 801698a9b6..f419f37b6b 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.Trigger; import java.util.Optional; +import java.util.OptionalInt; import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; @@ -65,7 +66,8 @@ public class AutoTrajectory { private final Consumer resetOdometry; private final Consumer> controller; private final AllianceContext allianceCtx; - private final Timer timer = new Timer(); + private final Timer activeTimer = new Timer(); + private final Timer inactiveTimer = new Timer(); private final Subsystem driveSubsystem; private final AutoRoutine routine; @@ -142,7 +144,9 @@ private void logTrajectory(boolean starting) { } private void cmdInitialize() { - timer.restart(); + activeTimer.start(); + inactiveTimer.stop(); + inactiveTimer.reset(); isActive = true; isCompleted = false; logTrajectory(true); @@ -155,7 +159,7 @@ private void cmdExecute() { allianceNotReady.set(true); return; } - var sampleOpt = trajectory.sampleAt(timer.get(), allianceCtx.doFlip()); + var sampleOpt = trajectory.sampleAt(activeTimer.get(), allianceCtx.doFlip()); if (sampleOpt.isEmpty()) { return; } @@ -170,7 +174,9 @@ private void cmdExecute() { } private void cmdEnd(boolean interrupted) { - timer.stop(); + activeTimer.stop(); + activeTimer.reset(); + inactiveTimer.start(); isActive = false; isCompleted = !interrupted; cmdExecute(); // will force the controller to be fed the final sample @@ -179,7 +185,7 @@ private void cmdEnd(boolean interrupted) { } private boolean cmdIsFinished() { - return timer.get() > trajectory.getTotalTime() || !routine.isActive; + return activeTimer.get() > trajectory.getTotalTime() || !routine.isActive; } /** @@ -307,18 +313,60 @@ public Trigger inactive() { return active().negate(); } - /** - * Returns a trigger that rises to true a number of cycles after the trajectory ends and falls - * after one pulse. - * - * @param cycles The number of cycles to delay the trigger from rising to true. - * @return A trigger that is true when the trajectory is finished. - * @see #doneDelayed(int) - * @deprecated Use {@link #doneDelayed(int)} instead. - */ - @Deprecated(forRemoval = true, since = "2025") - public Trigger done(int cycles) { - return doneDelayed(cycles); + private Trigger timeTrigger(double targetTime, Timer timer) { + // Make the trigger only be high for 1 cycle when the time has elapsed + return new Trigger( + routine.loop(), + new BooleanSupplier() { + double lastTimestamp = -1.0; + OptionalInt pollTarget = OptionalInt.empty(); + + public boolean getAsBoolean() { + if (!timer.isRunning()) { + lastTimestamp = -1.0; + pollTarget = OptionalInt.empty(); + return false; + } + double nowTimestamp = timer.get(); + try { + boolean timeAligns = lastTimestamp < targetTime && nowTimestamp >= targetTime; + if (pollTarget.isEmpty() && timeAligns) { + // if the time aligns for this cycle and it hasn't aligned previously this cycle + pollTarget = OptionalInt.of(routine.pollCount()); + return true; + } else if (pollTarget.isPresent() && routine.pollCount() == pollTarget.getAsInt()) { + // if the time aligned previously this cycle + return true; + } else if (pollTarget.isPresent()) { + // if the time aligned last cycle + pollTarget = OptionalInt.empty(); + return false; + } + return false; + } finally { + lastTimestamp = nowTimestamp; + } + } + }); + } + + private Trigger enterExitTrigger(Trigger enter, Trigger exit) { + return new Trigger( + routine.loop(), + new BooleanSupplier() { + boolean output = false; + + @Override + public boolean getAsBoolean() { + if (enter.getAsBoolean()) { + output = true; + } + if (exit.getAsBoolean()) { + output = false; + } + return output; + } + }); } /** @@ -354,39 +402,25 @@ public Trigger done(int cycles) { * // to trigger goShootGamepiece.cmd() even if we no longer care about these checks * * - * @param cycles The number of cycles to delay the trigger from rising to true. + * @param seconds The seconds to delay the trigger from rising to true. * @return A trigger that is true when the trajectory is finished. */ - public Trigger doneDelayed(int cycles) { - BooleanSupplier checker = - new BooleanSupplier() { - /** The last used value for trajectory completeness */ - boolean lastCompleteness = false; - - /** The cycle to be true for */ - int cycleTarget = -1; + public Trigger doneDelayed(double seconds) { + return timeTrigger(seconds, inactiveTimer).and(new Trigger(routine.loop(), () -> isCompleted)); + } - @Override - public boolean getAsBoolean() { - if (!isCompleted || isActive) { - // update last seen value - lastCompleteness = false; - cycleTarget = -1; - return false; - } - if (isCompleted) { - // if just flipped to completed update last seen value - // and store the cycleTarget based of the current cycle - if (!lastCompleteness) { - cycleTarget = routine.pollCount() + cycles; - } - lastCompleteness = true; - } - // finally if check the cycle matches the target - return routine.pollCount() == cycleTarget; - } - }; - return new Trigger(routine.loop(), checker); + /** + * Returns a trigger that rises to true a number of cycles after the trajectory ends and falls + * after one pulse. + * + * @param cycles The number of cycles to delay the trigger from rising to true. + * @return A trigger that is true when the trajectory is finished. + * @see #doneDelayed(int) + * @deprecated Use {@link #doneDelayed(int)} instead. + */ + @Deprecated(forRemoval = true, since = "2025") + public Trigger done(int cycles) { + return doneDelayed(0.02 * cycles); } /** @@ -433,25 +467,8 @@ public Trigger done() { * @param cycles The number of cycles to stay true after the trajectory ends. * @return A trigger that stays true for a number of cycles after the trajectory ends. */ - public Trigger doneFor(int cycles) { - BooleanSupplier enterPulse = doneDelayed(0); - BooleanSupplier exitPulse = doneDelayed(cycles + 1); - BooleanSupplier checker = - new BooleanSupplier() { - boolean output = false; - - @Override - public boolean getAsBoolean() { - if (enterPulse.getAsBoolean()) { - output = true; - } - if (exitPulse.getAsBoolean()) { - output = false; - } - return output && isCompleted; - } - }; - return new Trigger(routine.loop(), checker); + public Trigger doneFor(double seconds) { + return enterExitTrigger(doneDelayed(0), doneDelayed(seconds)); } /** @@ -460,24 +477,7 @@ public boolean getAsBoolean() { * @return A trigger that is true when the trajectory was the last one active and is done. */ public Trigger recentlyDone() { - BooleanSupplier enterPulse = doneDelayed(0); - BooleanSupplier exitPulse = routine.idle().negate(); - BooleanSupplier checker = - new BooleanSupplier() { - boolean output = false; - - @Override - public boolean getAsBoolean() { - if (enterPulse.getAsBoolean()) { - output = true; - } - if (exitPulse.getAsBoolean()) { - output = false; - } - return output && isCompleted; - } - }; - return new Trigger(routine.loop(), checker); + return enterExitTrigger(doneDelayed(0), routine.idle().negate()); } /** @@ -508,23 +508,7 @@ public Trigger atTime(double timeSinceStart) { return offTrigger; } - // Make the trigger only be high for 1 cycle when the time has elapsed, - // this is needed for better support of multi-time triggers for multi events - return new Trigger( - routine.loop(), - new BooleanSupplier() { - double lastTimestamp = timer.get(); - - public boolean getAsBoolean() { - double nowTimestamp = timer.get(); - try { - return lastTimestamp < nowTimestamp && nowTimestamp >= timeSinceStart; - } finally { - lastTimestamp = nowTimestamp; - } - } - }) - .and(active()); + return timeTrigger(timeSinceStart, activeTimer); } /** diff --git a/choreolib/src/test/java/choreo/auto/DoneTest.java b/choreolib/src/test/java/choreo/auto/DoneTest.java index 61fc111b3f..7edc6ebe8f 100644 --- a/choreolib/src/test/java/choreo/auto/DoneTest.java +++ b/choreolib/src/test/java/choreo/auto/DoneTest.java @@ -40,12 +40,17 @@ public void testExecution() { AutoRoutine routine = factory.newRoutine("test"); AutoTrajectory traj = factory.trajectory(trajectory, routine, true); + Trigger oneSecondIn = traj.atTime(1.0); + Trigger twoSecondIn = traj.atTime(2.0); + Trigger done = traj.done(); - Trigger doneDelayed = traj.doneDelayed(2); - Trigger doneFor = traj.doneFor(2); + Trigger doneDelayed = traj.doneDelayed(2.0); + Trigger doneFor = traj.doneFor(2.5); Trigger recentlyDone = traj.recentlyDone(); // makes the scheduler poll the triggers every cycle + oneSecondIn.onTrue(Commands.none()); + twoSecondIn.onTrue(Commands.none()); done.onTrue(Commands.none()); doneDelayed.onTrue(Commands.none()); doneFor.onTrue(Commands.none()); @@ -60,6 +65,13 @@ public void testExecution() { DriverStation.refreshData(); assertTrue(DriverStation.isAutonomousEnabled()); + assertFalse(oneSecondIn); + assertFalse(twoSecondIn); + assertFalse(done); + assertFalse(doneDelayed); + assertFalse(doneFor); + assertFalse(recentlyDone); + scheduler.schedule(routine.cmd()); scheduler.schedule(traj.cmd()); scheduler.run(); @@ -67,6 +79,8 @@ public void testExecution() { assertTrue(routine.active()); assertTrue(traj.active()); + assertFalse(oneSecondIn); + assertFalse(twoSecondIn); assertFalse(done); assertFalse(doneDelayed); assertFalse(doneFor); @@ -74,24 +88,32 @@ public void testExecution() { SimHooks.stepTiming(1.0); scheduler.run(); + assertTrue(oneSecondIn); + assertTrue(oneSecondIn); + assertFalse(twoSecondIn); SimHooks.stepTiming(1.0); scheduler.run(); + assertFalse(oneSecondIn); + assertTrue(twoSecondIn); SimHooks.stepTiming(1.05); scheduler.run(); assertTrue(traj.inactive()); - assertTrue(done); assertTrue(done); assertFalse(doneDelayed); assertTrue(doneFor); assertTrue(recentlyDone); + SimHooks.stepTiming(0.2); + assertTrue(done); + SimHooks.stepTiming(1.0); scheduler.run(); assertFalse(done); assertFalse(doneDelayed); assertTrue(doneFor); assertTrue(recentlyDone); + SimHooks.stepTiming(1.0); scheduler.run(); assertFalse(done); assertTrue(doneDelayed); @@ -100,6 +122,7 @@ public void testExecution() { assertTrue(recentlyDone); routine.updateIdle(false); // simulating to starting a new trajectory + SimHooks.stepTiming(1.0); scheduler.run(); assertFalse(doneFor); assertFalse(recentlyDone); @@ -121,5 +144,11 @@ public void testExecution() { assertTrue(done); SimHooks.resumeTiming(); + + DriverStationSim.setDsAttached(false); + DriverStationSim.setEnabled(false); + DriverStationSim.setAutonomous(false); + DriverStationSim.notifyNewData(); + DriverStation.refreshData(); } } From 213fb7a905b716b43d7b88f186c6c0f693909915 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Wed, 22 Jan 2025 23:09:18 -0500 Subject: [PATCH 15/21] fixed test issues --- .../main/java/choreo/auto/AutoRoutine.java | 2 +- .../src/test/java/choreo/auto/DoneTest.java | 2 +- .../auto/RoutineKillNoAllianceTest.java | 22 +++++++------------ 3 files changed, 10 insertions(+), 16 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index be5975dac5..0f5a9db715 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -39,7 +39,7 @@ public class AutoRoutine { private final String name; /** The alliance helper that is used to determine flipping logic */ - private final AllianceContext allianceCtx; + final AllianceContext allianceCtx; /** A boolean utilized in {@link #active()} to resolve trueness */ boolean isActive = false; diff --git a/choreolib/src/test/java/choreo/auto/DoneTest.java b/choreolib/src/test/java/choreo/auto/DoneTest.java index 7edc6ebe8f..7d38428a49 100644 --- a/choreolib/src/test/java/choreo/auto/DoneTest.java +++ b/choreolib/src/test/java/choreo/auto/DoneTest.java @@ -91,7 +91,7 @@ public void testExecution() { assertTrue(oneSecondIn); assertTrue(oneSecondIn); assertFalse(twoSecondIn); - SimHooks.stepTiming(1.0); + SimHooks.stepTiming(1.1); scheduler.run(); assertFalse(oneSecondIn); assertTrue(twoSecondIn); diff --git a/choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java b/choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java index 48f23370fd..76653ca1de 100644 --- a/choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java +++ b/choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java @@ -8,8 +8,6 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.SchedulerMaker; import java.util.Optional; import java.util.function.Supplier; import org.junit.jupiter.api.BeforeEach; @@ -18,61 +16,57 @@ public class RoutineKillNoAllianceTest { AutoFactory factoryFlip; AutoFactory factoryNoFlip; - CommandScheduler scheduler = SchedulerMaker.make(); Supplier routineFlip = () -> factoryFlip.newRoutine("testRoutineKill"); Supplier routineNoFlip = () -> factoryNoFlip.newRoutine("testRoutineKill"); @BeforeEach void setup() { assert HAL.initialize(500, 0); - scheduler.cancelAll(); factoryFlip = AutoTestHelper.factory(true); factoryNoFlip = AutoTestHelper.factory(false); } - void testRoutineKill( - CommandScheduler scheduler, Supplier routineSupplier, boolean expectKill) { + void testRoutineKill(Supplier routineSupplier, boolean expectKill) { AutoRoutine routine = routineSupplier.get(); assertFalse(routine.isKilled); - scheduler.schedule(routine.cmd()); + routine.cmd().initialize(); // don't need to run, this should kill on schedule/initialize assertEquals(expectKill, routine.isKilled); - scheduler.cancelAll(); } @Test void testUnFlippedEmpty() { setAlliance(Optional.empty()); - testRoutineKill(scheduler, routineNoFlip, false); + testRoutineKill(routineNoFlip, false); } @Test void testUnFlippedBlue() { setAlliance((Optional.of(Alliance.Blue))); - testRoutineKill(scheduler, routineNoFlip, false); + testRoutineKill(routineNoFlip, false); } @Test void testUnFlippedRed() { setAlliance(Optional.of(Alliance.Red)); - testRoutineKill(scheduler, routineNoFlip, false); + testRoutineKill(routineNoFlip, false); } @Test void testFlippedEmpty() { setAlliance(Optional.empty()); - testRoutineKill(scheduler, routineFlip, true); + testRoutineKill(routineFlip, true); } @Test void testFlippedBlue() { setAlliance(Optional.of(Alliance.Blue)); - testRoutineKill(scheduler, routineFlip, false); + testRoutineKill(routineFlip, false); } @Test void testFlippedRed() { setAlliance(Optional.of(Alliance.Red)); - testRoutineKill(scheduler, routineFlip, false); + testRoutineKill(routineFlip, false); } } From cffff1c0baa1013fd6520b63b7bfddada83ab103 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Wed, 22 Jan 2025 23:11:10 -0500 Subject: [PATCH 16/21] ran fmt --- .../main/java/choreo/auto/AutoRoutine.java | 4 +- .../main/java/choreo/auto/AutoTrajectory.java | 62 +++++++++---------- .../src/test/java/choreo/auto/DoneTest.java | 7 +-- 3 files changed, 35 insertions(+), 38 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index 0f5a9db715..7065bf5592 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -52,6 +52,7 @@ public class AutoRoutine { /** The amount of times the routine has been polled */ private int pollCount = 0; + /** The timestamp of the current cycle */ private double cycleTimestamp = 0; @@ -115,6 +116,7 @@ public Trigger observe(BooleanSupplier condition) { int pollCount() { return pollCount; } + double cycleTimestamp() { return cycleTimestamp; } @@ -260,7 +262,7 @@ public Trigger anyActive(AutoTrajectory trajectory, AutoTrajectory... trajectori /** * Creates a trigger that returns true when any of the trajectories given are inactive. - * + * *

This trigger will only return true if the routine is active. * * @param trajectory The first trajectory to watch. diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index f419f37b6b..fe1d4ead6b 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -316,38 +316,38 @@ public Trigger inactive() { private Trigger timeTrigger(double targetTime, Timer timer) { // Make the trigger only be high for 1 cycle when the time has elapsed return new Trigger( - routine.loop(), - new BooleanSupplier() { - double lastTimestamp = -1.0; - OptionalInt pollTarget = OptionalInt.empty(); - - public boolean getAsBoolean() { - if (!timer.isRunning()) { - lastTimestamp = -1.0; - pollTarget = OptionalInt.empty(); - return false; - } - double nowTimestamp = timer.get(); - try { - boolean timeAligns = lastTimestamp < targetTime && nowTimestamp >= targetTime; - if (pollTarget.isEmpty() && timeAligns) { - // if the time aligns for this cycle and it hasn't aligned previously this cycle - pollTarget = OptionalInt.of(routine.pollCount()); - return true; - } else if (pollTarget.isPresent() && routine.pollCount() == pollTarget.getAsInt()) { - // if the time aligned previously this cycle - return true; - } else if (pollTarget.isPresent()) { - // if the time aligned last cycle - pollTarget = OptionalInt.empty(); - return false; - } - return false; - } finally { - lastTimestamp = nowTimestamp; - } + routine.loop(), + new BooleanSupplier() { + double lastTimestamp = -1.0; + OptionalInt pollTarget = OptionalInt.empty(); + + public boolean getAsBoolean() { + if (!timer.isRunning()) { + lastTimestamp = -1.0; + pollTarget = OptionalInt.empty(); + return false; + } + double nowTimestamp = timer.get(); + try { + boolean timeAligns = lastTimestamp < targetTime && nowTimestamp >= targetTime; + if (pollTarget.isEmpty() && timeAligns) { + // if the time aligns for this cycle and it hasn't aligned previously this cycle + pollTarget = OptionalInt.of(routine.pollCount()); + return true; + } else if (pollTarget.isPresent() && routine.pollCount() == pollTarget.getAsInt()) { + // if the time aligned previously this cycle + return true; + } else if (pollTarget.isPresent()) { + // if the time aligned last cycle + pollTarget = OptionalInt.empty(); + return false; } - }); + return false; + } finally { + lastTimestamp = nowTimestamp; + } + } + }); } private Trigger enterExitTrigger(Trigger enter, Trigger exit) { diff --git a/choreolib/src/test/java/choreo/auto/DoneTest.java b/choreolib/src/test/java/choreo/auto/DoneTest.java index 7d38428a49..7973b1172f 100644 --- a/choreolib/src/test/java/choreo/auto/DoneTest.java +++ b/choreolib/src/test/java/choreo/auto/DoneTest.java @@ -18,20 +18,15 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SchedulerMaker; import edu.wpi.first.wpilibj2.command.button.Trigger; -import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; public class DoneTest { private static final Pose2d start = new Pose2d(); private static final Pose2d end = new Pose2d(2.0, 2.0, new Rotation2d(Math.PI)); - @BeforeEach - void setup() { - assert HAL.initialize(500, 0); - } - @Test public void testExecution() { + assert HAL.initialize(500, 0); CommandScheduler scheduler = SchedulerMaker.make(); AutoFactory factory = AutoTestHelper.factory(); From 800c4b92ecaa11843b0498da766604eec0eb44ba Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Wed, 22 Jan 2025 23:14:34 -0500 Subject: [PATCH 17/21] fixed javadocs --- choreolib/src/main/java/choreo/auto/AutoTrajectory.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index fe1d4ead6b..09afcc3be9 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -464,7 +464,7 @@ public Trigger done() { /** * Returns a trigger that stays true for a number of cycles after the trajectory ends. * - * @param cycles The number of cycles to stay true after the trajectory ends. + * @param seconds Seconds to stay true after the trajectory ends. * @return A trigger that stays true for a number of cycles after the trajectory ends. */ public Trigger doneFor(double seconds) { From f1b51f41647aa041e4d26f688bc7b03dab0ba783 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Wed, 29 Jan 2025 00:55:25 -0500 Subject: [PATCH 18/21] added test for trajectory cmd --- .../main/java/choreo/auto/AutoRoutine.java | 11 ++-- .../main/java/choreo/auto/AutoTrajectory.java | 2 +- .../test/java/choreo/auto/AutoTestHelper.java | 16 ++++-- .../java/choreo/auto/TrajectoryCmdTest.java | 54 +++++++++++++++++++ 4 files changed, 74 insertions(+), 9 deletions(-) create mode 100644 choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index 7065bf5592..70bfb2a88d 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -42,11 +42,16 @@ public class AutoRoutine { final AllianceContext allianceCtx; /** A boolean utilized in {@link #active()} to resolve trueness */ - boolean isActive = false; + private boolean isActive = false; + + private final Trigger isActiveTrigger = + new Trigger(loop, () -> isActive && DriverStation.isAutonomousEnabled()); /** A boolean indicating if a trajectory is running on the routine right now */ private boolean isIdle = true; + private final Trigger isIdleTrigger = new Trigger(loop, () -> isIdle); + /** A boolean that is true when the loop is killed */ boolean isKilled = false; @@ -79,7 +84,7 @@ public class AutoRoutine { * @return A {@link Trigger} that is true while this autonomous routine is being polled. */ public Trigger active() { - return new Trigger(loop, () -> isActive && DriverStation.isAutonomousEnabled()); + return isActiveTrigger; } /** Polls the routine. Should be called in the autonomous periodic method. */ @@ -160,7 +165,7 @@ public void kill() { * @return A trigger that is true when the routine is idle. */ public Trigger idle() { - return new Trigger(loop, () -> isIdle); + return isIdleTrigger; } /** diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index 09afcc3be9..02c63506c3 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -185,7 +185,7 @@ private void cmdEnd(boolean interrupted) { } private boolean cmdIsFinished() { - return activeTimer.get() > trajectory.getTotalTime() || !routine.isActive; + return activeTimer.get() > trajectory.getTotalTime() || !routine.active().getAsBoolean(); } /** diff --git a/choreolib/src/test/java/choreo/auto/AutoTestHelper.java b/choreolib/src/test/java/choreo/auto/AutoTestHelper.java index 1853551bfd..1fddcd36d2 100644 --- a/choreolib/src/test/java/choreo/auto/AutoTestHelper.java +++ b/choreolib/src/test/java/choreo/auto/AutoTestHelper.java @@ -12,17 +12,23 @@ import java.util.concurrent.atomic.AtomicReference; public class AutoTestHelper { - public static AutoFactory factory(boolean useAllianceFlipping) { - AtomicReference pose = new AtomicReference<>(new Pose2d()); + public static AutoFactory factory( + boolean useAllianceFlipping, AtomicReference robotPose) { + // AtomicReference pose = new AtomicReference<>(new Pose2d()); return new AutoFactory( - () -> pose.get(), - newPose -> pose.set(newPose), - sample -> pose.set(sample.getPose()), + () -> robotPose.get(), + newPose -> robotPose.set(newPose), + sample -> robotPose.set(sample.getPose()), useAllianceFlipping, new Subsystem() {}, (sample, isStart) -> {}); } + public static AutoFactory factory(boolean useAllianceFlipping) { + AtomicReference pose = new AtomicReference<>(new Pose2d()); + return factory(useAllianceFlipping, pose); + } + public static AutoFactory factory() { return factory(false); } diff --git a/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java b/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java new file mode 100644 index 0000000000..3fa77403f3 --- /dev/null +++ b/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java @@ -0,0 +1,54 @@ +// Copyright (c) Choreo contributors + +package choreo.auto; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import choreo.trajectory.SwerveSample; +import choreo.trajectory.Trajectory; +import choreo.trajectory.TrajectoryTestHelper; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.SchedulerMaker; +import java.util.concurrent.atomic.AtomicReference; +import org.junit.jupiter.api.Test; + +public class TrajectoryCmdTest { + private static final Pose2d start = new Pose2d(); + private static final Pose2d end = new Pose2d(2.0, 2.0, new Rotation2d(Math.PI)); + + @Test + public void testExecution() { + assert HAL.initialize(500, 0); + CommandScheduler scheduler = SchedulerMaker.make(); + AtomicReference pose = new AtomicReference<>(new Pose2d()); + AutoFactory factory = AutoTestHelper.factory(false, pose); + Trajectory trajectory = + TrajectoryTestHelper.linearTrajectory("test", start, end, 3.0, SwerveSample.class); + Command trajectoryCmd = factory.trajectoryCmd(trajectory); + + scheduler.schedule(trajectoryCmd); + + // trajectoryCmd should be scheduled for trajectory.getTotalTime() seconds and call the sample + // consumer on every cycle of that duration + + SimHooks.pauseTiming(); + for (int i = 0; i < 149; i++) { + scheduler.run(); + assertTrue(scheduler.isScheduled(trajectoryCmd)); + SimHooks.stepTiming(0.02); + } + + SimHooks.stepTiming(0.1); + scheduler.run(); + + assertFalse(scheduler.isScheduled(trajectoryCmd)); + + assertTrue(pose.get().getTranslation().getDistance(end.getTranslation()) < 0.1); + } +} From ccf64ebed0f5de6014c67d4dc56d6c6397362dac Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Wed, 29 Jan 2025 01:32:21 -0500 Subject: [PATCH 19/21] raaahhhhh --- .../main/java/choreo/auto/AutoTrajectory.java | 21 ++++++++++++++++-- .../java/choreo/trajectory/Trajectory.java | 4 ++-- .../src/test/java/choreo/auto/DoneTest.java | 7 +++--- .../java/choreo/auto/TrajectoryCmdTest.java | 22 ++++++++++++++++++- 4 files changed, 46 insertions(+), 8 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index 02c63506c3..5c58e4e6b3 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -173,19 +173,36 @@ private void cmdExecute() { } } + @SuppressWarnings("unchecked") private void cmdEnd(boolean interrupted) { activeTimer.stop(); activeTimer.reset(); inactiveTimer.start(); isActive = false; isCompleted = !interrupted; - cmdExecute(); // will force the controller to be fed the final sample + + if (!interrupted && allianceCtx.allianceKnownOrIgnored()) { + var sampleOpt = trajectory.getFinalSample(allianceCtx.doFlip()); + if (sampleOpt.isPresent()) { + var sample = sampleOpt.get(); + if (sample instanceof SwerveSample swerveSample) { + var swerveController = (Consumer) this.controller; + swerveController.accept(swerveSample); + } else if (sample instanceof DifferentialSample differentialSample) { + var differentialController = (Consumer) this.controller; + differentialController.accept(differentialSample); + } + } + } + logTrajectory(false); routine.updateIdle(true); } private boolean cmdIsFinished() { - return activeTimer.get() > trajectory.getTotalTime() || !routine.active().getAsBoolean(); + return activeTimer.get() > trajectory.getTotalTime() + || !routine.active().getAsBoolean() + || !allianceCtx.allianceKnownOrIgnored(); } /** diff --git a/choreolib/src/main/java/choreo/trajectory/Trajectory.java b/choreolib/src/main/java/choreo/trajectory/Trajectory.java index cb19b15738..286eeefb5d 100644 --- a/choreolib/src/main/java/choreo/trajectory/Trajectory.java +++ b/choreolib/src/main/java/choreo/trajectory/Trajectory.java @@ -155,8 +155,8 @@ private Optional sampleInternal(double timestamp) { * @return The SampleType at the given time. */ public Optional sampleAt(double timestamp, boolean mirrorForRedAlliance) { - Optional state = sampleInternal(timestamp); - return mirrorForRedAlliance ? state.map(SampleType::flipped) : state; + Optional sample = sampleInternal(timestamp); + return mirrorForRedAlliance ? sample.map(SampleType::flipped) : sample; } /** diff --git a/choreolib/src/test/java/choreo/auto/DoneTest.java b/choreolib/src/test/java/choreo/auto/DoneTest.java index 7973b1172f..b44cd73688 100644 --- a/choreolib/src/test/java/choreo/auto/DoneTest.java +++ b/choreolib/src/test/java/choreo/auto/DoneTest.java @@ -140,10 +140,11 @@ public void testExecution() { SimHooks.resumeTiming(); - DriverStationSim.setDsAttached(false); - DriverStationSim.setEnabled(false); - DriverStationSim.setAutonomous(false); + DriverStationSim.setDsAttached(true); + DriverStationSim.setEnabled(true); + DriverStationSim.setAutonomous(true); DriverStationSim.notifyNewData(); DriverStation.refreshData(); + assertTrue(DriverStation.isAutonomousEnabled()); } } diff --git a/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java b/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java index 3fa77403f3..7eb2171d0b 100644 --- a/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java +++ b/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java @@ -11,6 +11,8 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj.simulation.SimHooks; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -30,6 +32,7 @@ public void testExecution() { AutoFactory factory = AutoTestHelper.factory(false, pose); Trajectory trajectory = TrajectoryTestHelper.linearTrajectory("test", start, end, 3.0, SwerveSample.class); + Command trajectoryCmd = factory.trajectoryCmd(trajectory); scheduler.schedule(trajectoryCmd); @@ -38,6 +41,14 @@ public void testExecution() { // consumer on every cycle of that duration SimHooks.pauseTiming(); + + DriverStationSim.setDsAttached(true); + DriverStationSim.setEnabled(true); + DriverStationSim.setAutonomous(true); + DriverStationSim.notifyNewData(); + DriverStation.refreshData(); + assertTrue(DriverStation.isAutonomousEnabled()); + for (int i = 0; i < 149; i++) { scheduler.run(); assertTrue(scheduler.isScheduled(trajectoryCmd)); @@ -49,6 +60,15 @@ public void testExecution() { assertFalse(scheduler.isScheduled(trajectoryCmd)); - assertTrue(pose.get().getTranslation().getDistance(end.getTranslation()) < 0.1); + assertTrue(pose.get().getTranslation().getDistance(end.getTranslation()) < 0.5); + + DriverStationSim.setDsAttached(true); + DriverStationSim.setEnabled(true); + DriverStationSim.setAutonomous(true); + DriverStationSim.notifyNewData(); + DriverStation.refreshData(); + assertTrue(DriverStation.isAutonomousEnabled()); + + SimHooks.resumeTiming(); } } From ed03583239278c34196a6fb0ba6906c7f13c636a Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Wed, 29 Jan 2025 01:39:13 -0500 Subject: [PATCH 20/21] added atTimeBeforeEnd --- .../src/main/java/choreo/auto/AutoTrajectory.java | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index 5c58e4e6b3..da38bc77ca 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -528,6 +528,17 @@ public Trigger atTime(double timeSinceStart) { return timeTrigger(timeSinceStart, activeTimer); } + /** + * Returns a trigger that will go true for 1 cycle when the desired before the + * end of the trajectory time. + * + * @param timeBeforeEnd The time before the end of the trajectory. + * @return A trigger that is true when timeBeforeEnd has elapsed. + */ + public Trigger atTimeBeforeEnd(double timeBeforeEnd) { + return atTime(trajectory.getTotalTime() - timeBeforeEnd); + } + /** * Returns a trigger that is true when the event with the given name has been reached based on * time. From e9efa638d6fd82dcfa4a6bb3b63d313e82a4f1f0 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Wed, 29 Jan 2025 01:48:19 -0500 Subject: [PATCH 21/21] ran cfg --- choreolib/src/main/java/choreo/auto/AutoTrajectory.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index da38bc77ca..b9a2884e84 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -529,9 +529,9 @@ public Trigger atTime(double timeSinceStart) { } /** - * Returns a trigger that will go true for 1 cycle when the desired before the - * end of the trajectory time. - * + * Returns a trigger that will go true for 1 cycle when the desired before the end of the + * trajectory time. + * * @param timeBeforeEnd The time before the end of the trajectory. * @return A trigger that is true when timeBeforeEnd has elapsed. */