diff --git a/choreolib/src/main/java/choreo/auto/AutoFactory.java b/choreolib/src/main/java/choreo/auto/AutoFactory.java index 9e653cfa59..9e121309de 100644 --- a/choreolib/src/main/java/choreo/auto/AutoFactory.java +++ b/choreolib/src/main/java/choreo/auto/AutoFactory.java @@ -150,7 +150,7 @@ public void reset() {} @Override public Trigger active() { - return new Trigger(this.loop(), () -> false); + return new Trigger(this.loop(), () -> true); } }; } @@ -207,7 +207,7 @@ public AutoRoutine newRoutine(String name) { * * @see AutoRoutine#trajectory(String) */ - AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine) { + AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine, boolean useBindings) { Optional> optTrajectory = trajectoryCache.loadTrajectory(trajectoryName); Trajectory trajectory; @@ -216,7 +216,7 @@ AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine) { } else { trajectory = new Trajectory(trajectoryName, List.of(), List.of(), List.of()); } - return trajectory(trajectory, routine); + return trajectory(trajectory, routine, useBindings); } /** @@ -225,7 +225,8 @@ AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine) { * * @see AutoRoutine#trajectory(String, int) */ - AutoTrajectory trajectory(String trajectoryName, final int splitIndex, AutoRoutine routine) { + AutoTrajectory trajectory( + String trajectoryName, final int splitIndex, AutoRoutine routine, boolean useBindings) { Optional> optTrajectory = trajectoryCache.loadTrajectory(trajectoryName, splitIndex); Trajectory trajectory; @@ -234,7 +235,7 @@ AutoTrajectory trajectory(String trajectoryName, final int splitIndex, AutoRouti } else { trajectory = new Trajectory(trajectoryName, List.of(), List.of(), List.of()); } - return trajectory(trajectory, routine); + return trajectory(trajectory, routine, useBindings); } /** @@ -245,7 +246,7 @@ AutoTrajectory trajectory(String trajectoryName, final int splitIndex, AutoRouti */ @SuppressWarnings("unchecked") > AutoTrajectory trajectory( - Trajectory trajectory, AutoRoutine routine) { + Trajectory trajectory, AutoRoutine routine, boolean useBindings) { // type solidify everything final Trajectory solidTrajectory = trajectory; final Consumer solidController = (Consumer) this.controller; @@ -259,7 +260,7 @@ > AutoTrajectory trajectory( (TrajectoryLogger) trajectoryLogger, driveSubsystem, routine, - bindings); + useBindings ? bindings : new AutoBindings()); } /** @@ -277,10 +278,7 @@ > AutoTrajectory trajectory( * @return A new {@link AutoTrajectory}. */ public Command trajectoryCmd(String trajectoryName) { - AutoRoutine routine = newRoutine("Routine"); - AutoTrajectory trajectory = routine.trajectory(trajectoryName); - routine.active().onTrue(trajectory.cmd()); - return routine.cmd().until(trajectory.done()); + return trajectory(trajectoryName, voidRoutine, false).cmd(); } /** @@ -299,10 +297,7 @@ public Command trajectoryCmd(String trajectoryName) { * @return A new {@link AutoTrajectory}. */ public Command trajectoryCmd(String trajectoryName, final int splitIndex) { - AutoRoutine routine = newRoutine("Routine"); - AutoTrajectory trajectory = routine.trajectory(trajectoryName, splitIndex); - routine.active().onTrue(trajectory.cmd()); - return routine.cmd().until(trajectory.done()); + return trajectory(trajectoryName, splitIndex, voidRoutine, false).cmd(); } /** @@ -322,7 +317,7 @@ public Command trajectoryCmd(String trajectoryName, final int splitIndex) { * @return A new {@link AutoTrajectory}. */ public > Command trajectoryCmd(Trajectory trajectory) { - return trajectory(trajectory, voidRoutine).cmd(); + return trajectory(trajectory, voidRoutine, false).cmd(); } /** @@ -332,7 +327,7 @@ public > Command trajectoryCmd(Trajectory tr * @return A command that resets the robot's odometry. */ public Command resetOdometry(String trajectoryName) { - return trajectory(trajectoryName, voidRoutine).resetOdometry(); + return trajectory(trajectoryName, voidRoutine, false).resetOdometry(); } /** @@ -343,7 +338,7 @@ public Command resetOdometry(String trajectoryName) { * @return A command that resets the robot's odometry. */ public Command resetOdometry(String trajectoryName, final int splitIndex) { - return trajectory(trajectoryName, splitIndex, voidRoutine).resetOdometry(); + return trajectory(trajectoryName, splitIndex, voidRoutine, false).resetOdometry(); } /** @@ -355,7 +350,7 @@ public Command resetOdometry(String trajectoryName, final int splitIndex) { * @return A command that resets the robot's odometry. */ public > Command resetOdometry(Trajectory trajectory) { - return trajectory(trajectory, voidRoutine).resetOdometry(); + return trajectory(trajectory, voidRoutine, false).resetOdometry(); } /** diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index fa3abeedea..051ae916ec 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -41,7 +41,7 @@ public class AutoRoutine { private final AllianceContext allianceCtx; /** A boolean utilized in {@link #active()} to resolve trueness */ - private boolean isActive = false; + boolean isActive = false; /** A boolean that is true when the loop is killed */ boolean isKilled = false; @@ -142,7 +142,7 @@ public void kill() { * @return A new {@link AutoTrajectory}. */ public AutoTrajectory trajectory(String trajectoryName) { - return factory.trajectory(trajectoryName, this); + return factory.trajectory(trajectoryName, this, true); } /** @@ -153,7 +153,7 @@ public AutoTrajectory trajectory(String trajectoryName) { * @return A new {@link AutoTrajectory}. */ public AutoTrajectory trajectory(String trajectoryName, final int splitIndex) { - return factory.trajectory(trajectoryName, splitIndex, this); + return factory.trajectory(trajectoryName, splitIndex, this, true); } /** @@ -165,7 +165,7 @@ public AutoTrajectory trajectory(String trajectoryName, final int splitIndex) { */ public > AutoTrajectory trajectory( Trajectory trajectory) { - return factory.trajectory(trajectory, this); + return factory.trajectory(trajectory, this, true); } /** diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index b1b64063eb..0b01147a99 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -176,7 +176,7 @@ private void cmdEnd(boolean interrupted) { } private boolean cmdIsFinished() { - return timer.get() > trajectory.getTotalTime() || !routine.active().getAsBoolean(); + return timer.get() > trajectory.getTotalTime() || !routine.isActive; } /** diff --git a/choreolib/src/test/java/choreo/auto/DoneTest.java b/choreolib/src/test/java/choreo/auto/DoneTest.java index 7eddac8456..63f4bedc98 100644 --- a/choreolib/src/test/java/choreo/auto/DoneTest.java +++ b/choreolib/src/test/java/choreo/auto/DoneTest.java @@ -37,7 +37,7 @@ public void testExecution() { Trajectory trajectory = TrajectoryTestHelper.linearTrajectory("test", start, end, 3.0, SwerveSample.class); AutoRoutine routine = factory.newRoutine("test"); - AutoTrajectory traj = factory.trajectory(trajectory, routine); + AutoTrajectory traj = factory.trajectory(trajectory, routine, true); BooleanSupplier done = traj.done(); BooleanSupplier doneDelayed = traj.done(2);