Skip to content

Commit

Permalink
Make AutoRoutine command end immediately if alliance optional is empt…
Browse files Browse the repository at this point in the history 
…y on schedule. (#970)
  • Loading branch information
@shueja
shueja authored Dec 6, 2024
1 parent e99dc14 commit f4c70fb
Show file tree
Hide file tree
Showing 9 changed files with 498 additions and 77 deletions.
61 changes: 51 additions & 10 deletions choreolib/src/main/java/choreo/Choreo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
Expand Down Expand Up @@ -330,11 +331,11 @@ public void clear() {
* robot.
* @param controller A function that receives the current {@link SampleType} and controls the
* robot.
* @param mirrorTrajectory If this returns true, the path will be mirrored to the opposite side,
* while keeping the same coordinate system origin. This will be called every loop during the
* command.
* @param driveSubsystem The drive {@link Subsystem} to require for {@link AutoTrajectory} {@link
* Command}s.
* @param useAllianceFlipping If this returns true, when on the red alliance, the path will be
* mirrored to the opposite side, while keeping the same coordinate system origin. This will
* be called every loop during the command.
* @param bindings Universal trajectory event bindings.
* @return An {@link AutoFactory} that can be used to create {@link AutoRoutine} and {@link
* AutoTrajectory}.
Expand All @@ -343,14 +344,14 @@ public void clear() {
public static <SampleType extends TrajectorySample<SampleType>> AutoFactory createAutoFactory(
Supplier<Pose2d> poseSupplier,
Consumer<SampleType> controller,
BooleanSupplier mirrorTrajectory,
BooleanSupplier useAllianceFlipping,
Subsystem driveSubsystem,
AutoBindings bindings) {
return new AutoFactory(
requireNonNullParam(poseSupplier, "poseSupplier", "Choreo.createAutoFactory"),
requireNonNullParam(controller, "controller", "Choreo.createAutoFactory"),
requireNonNullParam(mirrorTrajectory, "mirrorTrajectory", "Choreo.createAutoFactory"),
requireNonNullParam(driveSubsystem, "driveSubsystem", "Choreo.createAutoFactory"),
requireNonNullParam(useAllianceFlipping, "useAllianceFlipping", "Choreo.createAutoFactory"),
requireNonNullParam(bindings, "bindings", "Choreo.createAutoFactory"),
Optional.empty());
}
Expand All @@ -363,11 +364,11 @@ public static <SampleType extends TrajectorySample<SampleType>> AutoFactory crea
* robot.
* @param controller A function that receives the current {@link SampleType} and controls the
* robot.
* @param mirrorTrajectory If this returns true, the path will be mirrored to the opposite side,
* while keeping the same coordinate system origin. This will be called every loop during the
* command.
* @param driveSubsystem The drive {@link Subsystem} to require for {@link AutoTrajectory} {@link
* Command}s.
* @param useAllianceFlipping If this returns true, when on the red alliance, the path will be
* mirrored to the opposite side, while keeping the same coordinate system origin. This will
* be called every loop during the command.
* @param bindings Universal trajectory event bindings.
* @param trajectoryLogger A {@link TrajectoryLogger} to log {@link Trajectory} as they start and
* finish.
Expand All @@ -378,16 +379,56 @@ public static <SampleType extends TrajectorySample<SampleType>> AutoFactory crea
public static <SampleType extends TrajectorySample<SampleType>> AutoFactory createAutoFactory(
Supplier<Pose2d> poseSupplier,
Consumer<SampleType> controller,
BooleanSupplier mirrorTrajectory,
BooleanSupplier useAllianceFlipping,
Subsystem driveSubsystem,
AutoBindings bindings,
TrajectoryLogger<SampleType> trajectoryLogger) {
return new AutoFactory(
requireNonNullParam(poseSupplier, "poseSupplier", "Choreo.createAutoFactory"),
requireNonNullParam(controller, "controller", "Choreo.createAutoFactory"),
requireNonNullParam(mirrorTrajectory, "mirrorTrajectory", "Choreo.createAutoFactory"),
requireNonNullParam(driveSubsystem, "driveSubsystem", "Choreo.createAutoFactory"),
requireNonNullParam(useAllianceFlipping, "useAllianceFlipping", "Choreo.createAutoFactory"),
requireNonNullParam(bindings, "bindings", "Choreo.createAutoFactory"),
Optional.of(trajectoryLogger));
}

/**
* Create a factory that can be used to create {@link AutoRoutine} and {@link AutoTrajectory}.
*
* @param <SampleType> The type of samples in the trajectory.
* @param poseSupplier A function that returns the current field-relative {@link Pose2d} of the
* robot.
* @param controller A function that receives the current {@link SampleType} and controls the
* robot.
* @param driveSubsystem The drive {@link Subsystem} to require for {@link AutoTrajectory} {@link
* Command}s.
* @param useAllianceFlipping If this returns true, when on the red alliance, the path will be
* mirrored to the opposite side, while keeping the same coordinate system origin. This will
* be called every loop during the command.
* @param bindings Universal trajectory event bindings.
* @param trajectoryLogger A {@link TrajectoryLogger} to log {@link Trajectory} as they start and
* finish.
* @param alliance A custom supplier of the current alliance to use instead of {@link
* DriverStation#getAlliance}.
* @return An {@link AutoFactory} that can be used to create {@link AutoRoutine} and {@link
* AutoTrajectory}.
* @see AutoChooser using this factory with AutoChooser to generate auto routines.
*/
public static <SampleType extends TrajectorySample<SampleType>> AutoFactory createAutoFactory(
Supplier<Pose2d> poseSupplier,
Consumer<SampleType> controller,
Subsystem driveSubsystem,
BooleanSupplier useAllianceFlipping,
AutoBindings bindings,
TrajectoryLogger<SampleType> trajectoryLogger,
Supplier<Optional<Alliance>> alliance) {
return new AutoFactory(
requireNonNullParam(poseSupplier, "poseSupplier", "Choreo.createAutoFactory"),
requireNonNullParam(controller, "controller", "Choreo.createAutoFactory"),
requireNonNullParam(driveSubsystem, "driveSubsystem", "Choreo.createAutoFactory"),
requireNonNullParam(useAllianceFlipping, "useAllianceFlipping", "Choreo.createAutoFactory"),
requireNonNullParam(bindings, "bindings", "Choreo.createAutoFactory"),
Optional.of(trajectoryLogger),
requireNonNullParam(alliance, "alliance", "Choreo.createAutoFactory"));
}
}
55 changes: 47 additions & 8 deletions choreolib/src/main/java/choreo/auto/AutoFactory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.event.EventLoop;
import edu.wpi.first.wpilibj2.command.Command;
Expand Down Expand Up @@ -96,40 +97,73 @@ HashMap<String, Command> getBindings() {
private final TrajectoryCache trajectoryCache = new TrajectoryCache();
private final Supplier<Pose2d> poseSupplier;
private final Consumer<? extends TrajectorySample<?>> controller;
private final BooleanSupplier mirrorTrajectory;
private final Supplier<Optional<Alliance>> alliance;
private final BooleanSupplier useAllianceFlipping;
private final Subsystem driveSubsystem;
private final AutoBindings bindings = new AutoBindings();
private final Optional<TrajectoryLogger<? extends TrajectorySample<?>>> trajectoryLogger;

/**
* Its recommended to use the {@link Choreo#createAutoFactory} to create a new instance of this
* It is recommended to use the {@link Choreo#createAutoFactory} to create a new instance of this
* class.
*
* @param <SampleType> {@link Choreo#createAutoFactory}
* @param poseSupplier {@link Choreo#createAutoFactory}
* @param controller {@link Choreo#createAutoFactory}
* @param mirrorTrajectory {@link Choreo#createAutoFactory}
* @param driveSubsystem {@link Choreo#createAutoFactory}
* @param useAllianceFlipping {@link Choreo#createAutoFactory}
* @param bindings {@link Choreo#createAutoFactory}
* @param trajectoryLogger {@link Choreo#createAutoFactory}
* @param alliance {@link Choreo#createAutoFactory}
*/
public <SampleType extends TrajectorySample<SampleType>> AutoFactory(
Supplier<Pose2d> poseSupplier,
Consumer<SampleType> controller,
BooleanSupplier mirrorTrajectory,
Subsystem driveSubsystem,
BooleanSupplier useAllianceFlipping,
AutoBindings bindings,
Optional<TrajectoryLogger<SampleType>> trajectoryLogger) {
Optional<TrajectoryLogger<SampleType>> trajectoryLogger,
Supplier<Optional<Alliance>> alliance) {
this.poseSupplier = poseSupplier;
this.controller = controller;
this.mirrorTrajectory = mirrorTrajectory;
this.driveSubsystem = driveSubsystem;
this.useAllianceFlipping = useAllianceFlipping;
this.bindings.merge(bindings);
this.trajectoryLogger =
trajectoryLogger.map(logger -> (TrajectoryLogger<? extends TrajectorySample<?>>) logger);
this.alliance = alliance;
HAL.report(tResourceType.kResourceType_ChoreoTrigger, 1);
}

/**
* It is recommended to use the {@link Choreo#createAutoFactory} to create a new instance of this
* class.
*
* @param <SampleType> {@link Choreo#createAutoFactory}
* @param poseSupplier {@link Choreo#createAutoFactory}
* @param controller {@link Choreo#createAutoFactory}
* @param driveSubsystem {@link Choreo#createAutoFactory}
* @param useAllianceFlipping {@link Choreo#createAutoFactory}
* @param bindings {@link Choreo#createAutoFactory}
* @param trajectoryLogger {@link Choreo#createAutoFactory}
*/
public <SampleType extends TrajectorySample<SampleType>> AutoFactory(
Supplier<Pose2d> poseSupplier,
Consumer<SampleType> controller,
Subsystem driveSubsystem,
BooleanSupplier useAllianceFlipping,
AutoBindings bindings,
Optional<TrajectoryLogger<SampleType>> trajectoryLogger) {
this(
poseSupplier,
controller,
driveSubsystem,
useAllianceFlipping,
bindings,
trajectoryLogger,
DriverStation::getAlliance);
}

/**
* Creates a new {@link AutoRoutine}.
*
Expand All @@ -143,7 +177,11 @@ public AutoRoutine newRoutine(String name) {
trajectoryCache.clear();
}

return new AutoRoutine(name);
return new AutoRoutine(name, this::allianceKnownOrIgnored);
}

private boolean allianceKnownOrIgnored() {
return !useAllianceFlipping.getAsBoolean() || alliance.get().isPresent();
}

/**
Expand Down Expand Up @@ -220,7 +258,8 @@ public <SampleType extends TrajectorySample<SampleType>> AutoTrajectory trajecto
solidTrajectory,
poseSupplier,
solidController,
mirrorTrajectory,
useAllianceFlipping,
alliance,
solidLogger,
driveSubsystem,
routine,
Expand Down
51 changes: 41 additions & 10 deletions choreolib/src/main/java/choreo/auto/AutoRoutine.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,9 @@ public class AutoRoutine {
/** The amount of times the routine has been polled */
protected int pollCount = 0;

/** Returns true if the alliance is known or is irrelevant (i.e. flipping is not being done) */
protected BooleanSupplier allianceKnownOrIgnored = () -> true;

/**
* Creates a new loop with a specific name
*
Expand All @@ -45,6 +48,20 @@ public AutoRoutine(String name) {
this.name = name;
}

/**
* Creates a new loop with a specific name and a custom alliance supplier.
*
* @param name The name of the loop
* @param allianceKnownOrIgnored Returns true if the alliance is known or is irrelevant (i.e.
* flipping is not being done).
* @see AutoFactory#newRoutine Creating a loop from a AutoFactory
*/
public AutoRoutine(String name, BooleanSupplier allianceKnownOrIgnored) {
this.loop = new EventLoop();
this.name = name;
this.allianceKnownOrIgnored = allianceKnownOrIgnored;
}

/**
* A constructor to be used when inhereting this class to instantiate a custom inner loop
*
Expand All @@ -70,7 +87,9 @@ public Trigger running() {

/** Polls the routine. Should be called in the autonomous periodic method. */
public void poll() {
if (!DriverStation.isAutonomousEnabled() || isKilled) {
if (!DriverStation.isAutonomousEnabled()
|| !allianceKnownOrIgnored.getAsBoolean()
|| isKilled) {
isActive = false;
return;
}
Expand Down Expand Up @@ -121,27 +140,39 @@ public void kill() {
/**
* Creates a command that will poll this event loop and reset it when it is cancelled.
*
* <p>The command will end instantly and kill the routine if the alliance supplier returns an
* empty optional when the command is scheduled.
*
* @return A command that will poll this event loop and reset it when it is cancelled.
* @see #cmd(BooleanSupplier) A version of this method that takes a condition to finish the loop.
*/
public Command cmd() {
return Commands.run(this::poll)
.finallyDo(this::reset)
.until(() -> !DriverStation.isAutonomousEnabled())
.withName(name);
return cmd(() -> false);
}

/**
* Creates a command that will poll this event loop and reset it when it is finished or canceled.
*
* <p>The command will end instantly and kill the routine if the alliance supplier returns an
* empty optional when the command is scheduled.
*
* @param finishCondition A condition that will finish the loop when it is true.
* @return A command that will poll this event loop and reset it when it is finished or canceled.
* @see #cmd() A version of this method that doesn't take a condition and never finishes.
* @see #cmd() A version of this method that doesn't take a condition and never finishes except if
* the alliance supplier returns an empty optional when scheduled.
*/
public Command cmd(BooleanSupplier finishCondition) {
return Commands.run(this::poll)
.finallyDo(this::reset)
.until(() -> !DriverStation.isAutonomousEnabled() || finishCondition.getAsBoolean())
.withName(name);
return Commands.either(
Commands.run(this::poll)
.finallyDo(this::reset)
.until(() -> !DriverStation.isAutonomousEnabled() || finishCondition.getAsBoolean())
.withName(name),
Commands.runOnce(
() -> {
DriverStation.reportWarning(
"[Choreo] Alliance not known when starting routine", false);
kill();
}),
allianceKnownOrIgnored);
}
}
Loading

0 comments on commit f4c70fb

Please sign in to comment.