diff --git a/choreolib/src/main/java/choreo/Choreo.java b/choreolib/src/main/java/choreo/Choreo.java index 5754d53e8..2d8cedb89 100644 --- a/choreolib/src/main/java/choreo/Choreo.java +++ b/choreolib/src/main/java/choreo/Choreo.java @@ -92,7 +92,7 @@ public static ProjectFile getProjectFile() { } catch (FileNotFoundException ex) { throw new RuntimeException("Could not find project file", ex); } catch (IOException ex) { - throw new RuntimeException("Could not find project file", ex); + throw new RuntimeException("Could not close the project file", ex); } return LAZY_PROJECT_FILE.get(); } diff --git a/choreolib/src/main/java/choreo/auto/AutoChooser.java b/choreolib/src/main/java/choreo/auto/AutoChooser.java index b3f9f2ad4..240a4442f 100644 --- a/choreolib/src/main/java/choreo/auto/AutoChooser.java +++ b/choreolib/src/main/java/choreo/auto/AutoChooser.java @@ -13,13 +13,16 @@ import java.util.function.Function; /** - * An auto chooser that allows for the selection of auto routines at runtime. + * An Choreo specific {@code SendableChooser} that allows for the selection of {@link AutoRoutine}s + * at runtime via a Dashboard. * - *

This chooser takes a lazy loading approach to auto routines, only generating the auto routine - * when it is selected. This approach has the benefit of not loading all autos on startup, but also - * not loading the auto during auto start causing a delay. + *

This chooser takes a lazy loading + * approach to {@link AutoRoutine}s, only generating the {@link AutoRoutine} when it is selected. + * This approach has the benefit of not loading all autos on startup, but also not loading the auto + * during auto start causing a delay. * - *

Once the {@link AutoChooser} is made you can add auto routines to it using the {@link + *

Once the {@link AutoChooser} is made you can add {@link AutoRoutine}s to it using the {@link * #addAutoRoutine(String, AutoRoutineGenerator)} method. Unlike {@code SendableChooser} this * chooser has to be updated every cycle by calling the {@link #update()} method in your {@link * IterativeRobotBase#robotPeriodic()}. @@ -48,15 +51,18 @@ public static interface AutoRoutineGenerator extends FunctionThis method should be called every cycle in the {@link IterativeRobotBase#robotPeriodic()}. - * It will check if the selected auto routine has changed and update the active auto routine. + * It will check if the selected auto routine has changed and update the active AutoRoutine. + * + *

The AutoRoutine can only be updated when the robot is disabled. If the chooser in your + * dashboard says {@code BAD} the {@link AutoChooser} has not responded to the selection yet and + * you need to disable the robot to update it. */ public void update() { - if (DriverStation.isDisabled() || IterativeRobotBase.isSimulation()) { + if (DriverStation.isDisabled()) { String selectStr = selected.get(); if (selectStr.equals(lastAutoRoutineName)) return; if (!autoRoutines.containsKey(selectStr)) { @@ -90,14 +100,33 @@ public void update() { } /** - * Add an auto routine to the chooser. + * Add an AutoRoutine to the chooser. + * + *

The options of the chooser are actually of type {@link AutoRoutineGenerator}. This is a + * function that takes an {@link AutoFactory} and returns a {@link AutoRoutine}. These functions + * can be static, a lambda or belong to a local variable. + * + *

This is done to load AutoRoutines when and only when they are selected, in order to save + * memory and file loading time for unused AutoRoutines. * - *

An auto routine is a function that takes an AutoFactory and returns a AutoRoutine. These - * functions can be static, a lambda or belong to a local variable. + *

One way to keep this clean is to make an `Autos` class that all of your subsystems/resources + * are dependency injected into. + * Then create methods inside that class that take an {@link AutoFactory} and return an {@link + * AutoRoutine}. * - *

A good paradigm is making an `AutoRoutines` class that has a reference to all your - * subsystems and has helper methods for auto commands inside it. Then you crate methods inside - * that class that take an `AutoFactory` and return a `Command`. + *

Example:

+ * + * 

+   * AutoChooser chooser;
+   * Autos autos = new Autos(swerve, shooter, intake, feeder);
+   * Robot() {
+   *   chooser = new AutoChooser(Choreo.createAutoFactory(...), "/Choosers");
+   *   chooser.addAutoRoutine("4 Piece right", autos::fourPieceRight);
+   *   chooser.addAutoRoutine("4 Piece Left", autos::fourPieceLeft);
+   *   chooser.addAutoRoutine("3 Piece Close", autos::threePieceClose);
+   *   chooser.addAutoRoutine("Just Shoot", factory -> factory.commandAsAutoRoutine(shooter.shoot()));
+   * }
+   *
* * @param name The name of the auto routine. * @param generator The function that generates the auto routine. @@ -108,9 +137,40 @@ public void addAutoRoutine(String name, AutoRoutineGenerator generator) { } /** - * Get the currently selected auto routine. + * Get the currently selected {@link AutoRoutine}. + * + *

Recommended Usage

+ * + * Scheduling it as a command. + * + * 

+   * AutoChooser chooser = ...;
+   *
+   * public void autonomousInit() {
+   *   CommandScheduler.getInstance().schedule(chooser.getSelectedAutoRoutine().cmd());
+   * }
+   *
+ * + * Polling it yourself. + * + * 

+   * AutoChooser chooser = ...;
+   * AutoRoutine routine = chooser.getSelectedAutoRoutine();
+   *
+   * public void autonomousInit() {
+   *   routine = chooser.getSelectedAutoRoutine();
+   * }
+   *
+   * public void autonomousPeriodic() {
+   *  routine.poll();
+   * }
+   *
+   * public void autonomousExit() {
+   *   routine.reset();
+   * }
+   *
* - * @return The currently selected auto routine. + * @return The currently selected {@link AutoRoutine}. */ public AutoRoutine getSelectedAutoRoutine() { return lastAutoRoutine; diff --git a/choreolib/src/main/java/choreo/auto/AutoFactory.java b/choreolib/src/main/java/choreo/auto/AutoFactory.java index b34915996..b3df53de8 100644 --- a/choreolib/src/main/java/choreo/auto/AutoFactory.java +++ b/choreolib/src/main/java/choreo/auto/AutoFactory.java @@ -24,43 +24,10 @@ import java.util.function.Supplier; /** - * A factory used to create autonomous routines. + * A factory used to create {@link AutoRoutine}s and {@link AutoTrajectory}s. * - *

Here is an example of how to use this class to create an auto routine: - * - *

Example using Triggers

- * - * 

- * public AutoRoutine shootThenMove(AutoFactory factory) {
- *   // Create a new auto routine to return
- *   var routine = factory.newRoutine();
- *
- *   // Create a trajectory that moves the robot 2 meters
- *   AutoTrajectory trajectory = factory.trajectory("move2meters", routine);
- *
- *   // Will automatically run the shoot command when the auto routine is first polled
- *   routine.enabled().onTrue(shooter.shoot());
- *
- *   // Gets a trigger from the shooter to if the shooter has a note, and will run the trajectory
- *   // command when the shooter does not have a note
- *   routine.enabled().and(shooter.hasNote()).onFalse(trajectory.cmd());
- *
- *   return routine;
- * }
- *
- * - *

Example using CommandGroups

- * - * 

- * public Command shootThenMove(AutoFactory factory) {
- *   // Create a trajectory that moves the robot 2 meters
- *   Command trajectory = factory.trajectoryCommand("move2meters");
- *
- *   return shooter.shoot()
- *      .andThen(trajectory)
- *      .withName("ShootThenMove");
- * }
- *
+ * @see Auto Routine + * Docs */ public class AutoFactory { static final AutoRoutine VOID_ROUTINE = @@ -166,7 +133,6 @@ public > AutoFactory( * * @param name The name of the {@link AutoRoutine}. * @return A new {@link AutoRoutine}. - * @see AutoRoutine * @see #voidRoutine */ public AutoRoutine newRoutine(String name) { @@ -190,11 +156,11 @@ public AutoRoutine voidRoutine() { } /** - * Creates a new auto trajectory to be used in an auto routine. + * Creates a new {@link AutoTrajectory} to be used in an auto routine. * * @param trajectoryName The name of the trajectory to use. * @param routine The {@link AutoRoutine} to register this trajectory under. - * @return A new auto trajectory. + * @return A new {@link AutoTrajectory}. */ public AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine) { Optional> optTrajectory = @@ -210,12 +176,12 @@ public AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine) { } /** - * Creates a new auto trajectory to be used in an auto routine. + * Creates a new {@link AutoTrajectory} to be used in an auto routine. * * @param trajectoryName The name of the trajectory to use. * @param splitIndex The index of the split trajectory to use. * @param routine The {@link AutoRoutine} to register this trajectory under. - * @return A new auto trajectory. + * @return A new {@link AutoTrajectory}. */ public AutoTrajectory trajectory( String trajectoryName, final int splitIndex, AutoRoutine routine) { @@ -232,12 +198,12 @@ public AutoTrajectory trajectory( } /** - * Creates a new auto trajectory to be used in an auto routine. + * Creates a new {@link AutoTrajectory} to be used in an auto routine. * * @param The type of the trajectory samples. * @param trajectory The trajectory to use. * @param routine The {@link AutoRoutine} to register this trajectory under. - * @return A new auto trajectory. + * @return A new {@link AutoTrajectory}. */ @SuppressWarnings("unchecked") public > AutoTrajectory trajectory( @@ -261,7 +227,7 @@ public > AutoTrajectory trajecto } /** - * Creates a new auto trajectory command to be used in an auto routine. + * Creates a new {@link AutoTrajectory} command to be used in an auto routine. * *

Important * @@ -272,14 +238,14 @@ public > AutoTrajectory trajecto * AutoBindings} passed into the factory constructor. * * @param trajectoryName The name of the trajectory to use. - * @return A new auto trajectory. + * @return A new {@link AutoTrajectory}. */ public Command trajectoryCommand(String trajectoryName) { return trajectory(trajectoryName, VOID_ROUTINE).cmd(); } /** - * Creates a new auto trajectory command to be used in an auto routine. + * Creates a new {@link AutoTrajectory} command to be used in an auto routine. * *

Important * @@ -291,14 +257,14 @@ public Command trajectoryCommand(String trajectoryName) { * * @param trajectoryName The name of the trajectory to use. * @param splitIndex The index of the split trajectory to use. - * @return A new auto trajectory. + * @return A new {@link AutoTrajectory}. */ public Command trajectoryCommand(String trajectoryName, final int splitIndex) { return trajectory(trajectoryName, splitIndex, VOID_ROUTINE).cmd(); } /** - * Creates a new auto trajectory command to be used in an auto routine. + * Creates a new {@link AutoTrajectory} command to be used in an auto routine. * *

Important * @@ -310,7 +276,7 @@ public Command trajectoryCommand(String trajectoryName, final int splitIndex) { * * @param The type of the trajectory samples. * @param trajectory The trajectory to use. - * @return A new auto trajectory. + * @return A new {@link AutoTrajectory}. */ public > Command trajectoryCommand( Trajectory trajectory) { diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index a26f7e1c6..a1717edd1 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -164,7 +164,7 @@ public Command cmd() { return driveSubsystem .runOnce( () -> { - DriverStation.reportError("Trajectory " + name + " has no samples", false); + DriverStation.reportError("[Choreo] Trajectory " + name + " has no samples", false); }) .withName("Trajectory_" + name); } @@ -355,14 +355,14 @@ public Trigger done() { public Trigger atTime(double timeSinceStart) { // The timer shhould never be negative so report this as a warning if (timeSinceStart < 0) { - DriverStation.reportWarning("Trigger time cannot be negative for " + name, true); + DriverStation.reportWarning("[Choreo] Trigger time cannot be negative for " + name, true); return offTrigger; } // The timer should never exceed the total trajectory time so report this as a warning if (timeSinceStart > trajectory.getTotalTime()) { DriverStation.reportWarning( - "Trigger time cannot be greater than total trajectory time for " + name, true); + "[Choreo] Trigger time cannot be greater than total trajectory time for " + name, true); return offTrigger; } @@ -394,6 +394,8 @@ public boolean getAsBoolean() { * @param eventName The name of the event. * @return A trigger that is true when the event with the given name has been reached based on * time. + * @see Event + * Markers in the GUI */ public Trigger atTime(String eventName) { boolean foundEvent = false; @@ -411,7 +413,8 @@ public Trigger atTime(String eventName) { // The user probably expects an event to exist if they're trying to do something at that event, // report the missing event. if (!foundEvent) { - DriverStation.reportWarning("Event \"" + eventName + "\" not found for " + name, true); + DriverStation.reportWarning( + "[Choreo] Event \"" + eventName + "\" not found for " + name, true); } return trig; @@ -451,6 +454,8 @@ public Trigger atPose(Pose2d pose, double toleranceMeters) { * @param toleranceMeters The tolerance in meters. * @return A trigger that is true when the robot is within toleranceMeters of the given events * pose. + * @see Event + * Markers in the GUI */ public Trigger atPose(String eventName, double toleranceMeters) { boolean foundEvent = false; @@ -469,7 +474,8 @@ public Trigger atPose(String eventName, double toleranceMeters) { // The user probably expects an event to exist if they're trying to do something at that event, // report the missing event. if (!foundEvent) { - DriverStation.reportWarning("Event \"" + eventName + "\" not found for " + name, true); + DriverStation.reportWarning( + "[Choreo] Event \"" + eventName + "\" not found for " + name, true); } return trig; @@ -483,6 +489,8 @@ public Trigger atPose(String eventName, double toleranceMeters) { * * @param eventName The name of the event. * @return A trigger that is true when the robot is within 3 inches of the given events pose. + * @see Event + * Markers in the GUI */ public Trigger atPose(String eventName) { return atPose(eventName, DEFAULT_TOLERANCE_METERS); @@ -499,6 +507,8 @@ public Trigger atPose(String eventName) { * @param toleranceMeters The tolerance in meters. * @return A trigger that is true when the event with the given name has been reached based on * time and the robot is within toleranceMeters of the given events pose. + * @see Event + * Markers in the GUI */ public Trigger atTimeAndPose(String eventName, double toleranceMeters) { return atTime(eventName).and(atPose(eventName, toleranceMeters)); @@ -514,6 +524,8 @@ public Trigger atTimeAndPose(String eventName, double toleranceMeters) { * @param eventName The name of the event. * @return A trigger that is true when the event with the given name has been reached based on * time and the robot is within 3 inches of the given events pose. + * @see Event + * Markers in the GUI */ public Trigger atTimeAndPose(String eventName) { return atTimeAndPose(eventName, DEFAULT_TOLERANCE_METERS); @@ -524,6 +536,8 @@ public Trigger atTimeAndPose(String eventName) { * * @param eventName The name of the event. * @return An array of all the timestamps of the events with the given name. + * @see Event + * Markers in the GUI */ public double[] collectEventTimes(String eventName) { return trajectory.getEvents(eventName).stream().mapToDouble(e -> e.timestamp).toArray(); @@ -534,6 +548,8 @@ public double[] collectEventTimes(String eventName) { * * @param eventName The name of the event. * @return An array of all the poses of the events with the given name. + * @see Event + * Markers in the GUI */ public Pose2d[] collectEventPoses(String eventName) { var times = collectEventTimes(eventName); diff --git a/choreolib/src/main/java/choreo/trajectory/Trajectory.java b/choreolib/src/main/java/choreo/trajectory/Trajectory.java index 524307f16..68555a125 100644 --- a/choreolib/src/main/java/choreo/trajectory/Trajectory.java +++ b/choreolib/src/main/java/choreo/trajectory/Trajectory.java @@ -82,7 +82,8 @@ public SampleType getInitialSample(boolean mirrorForRedAlliance) { if (samples.isEmpty()) { return null; } - return mirrorForRedAlliance ? samples.get(0).flipped() : samples.get(0); + final var sample = samples.get(0); + return mirrorForRedAlliance ? sample.flipped() : sample; } /** @@ -97,9 +98,8 @@ public SampleType getFinalSample(boolean mirrorForRedAlliance) { if (samples.isEmpty()) { return null; } - return mirrorForRedAlliance - ? samples.get(samples.size() - 1).flipped() - : samples.get(samples.size() - 1); + final var sample = samples.get(samples.size() - 1); + return mirrorForRedAlliance ? sample.flipped() : sample; } private SampleType sampleInternal(double timestamp) { diff --git a/docs/choreolib/auto-controller.md b/docs/choreolib/auto-controller.md new file mode 100644 index 000000000..a5716dcb9 --- /dev/null +++ b/docs/choreolib/auto-controller.md @@ -0,0 +1,69 @@ +# Auto Controller + +Choreo trajectory samples contain a lot of information. Due to this complexity, the +task of implementing how to follow these samples is left up to the user. + +The "Auto Controller" is a function that consumes a `Pose2d` as well as a sample (`SwerveSample` or `DifferentialSample` depending on drive base). +The controller should capture your drive subsystem to directly control it. + +## Basic PID Controller + +The most basic implementation of a vendor-independent auto controller is a PID controller. +This controller will take the base requested velocity from the sample and add the output of the PID controller to it. +The PID controller output is based on how far the robot is from the desired position. + +```java +public class AutoController implements BiConsumer { + private final Swerve swerve; // Swerve subsystem + private final PIDController xController = new PIDController( + kAuto.kTranslation.kP, + 0.0, + kAuto.kTranslation.kD + ); + private final PIDController yController = new PIDController( + kAuto.kTranslation.kP, + 0.0, + kAuto.kTranslation.kD + ); + private final PIDController headingController = new PIDController( + kAuto.kRotation.kP, + 0.0, + kAuto.kRotation.kD + ); + + public AutoController(Swerve swerve) { + this.swerve = swerve; + headingController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void accept(Pose2d pose, SwerveSample referenceState) { + double xFF = referenceState.vx; + double yFF = referenceState.vy; + double rotationFF = referenceState.omega; + + double xFeedback = xController.calculate(pose.getX(), referenceState.x); + double yFeedback = yController.calculate(pose.getY(), referenceState.y); + double rotationFeedback = headingController.calculate(pose.getRotation().getRadians(), + referenceState.heading); + + ChassisSpeeds out = ChassisSpeeds.fromFieldRelativeSpeeds( + xFF + xFeedback, + yFF + yFeedback, + rotationFF + rotationFeedback, + pose.getRotation() + ); + + swerve.drive(out); + } +} +``` + +## Advanced Controllers + +As a more advanced control strategy, you can utilize the acceleration from the sample to implement feedforward. You can also use the forces from the sample with the CTRE Swerve API or a custom implementation to better adhere to the model. + +Examples: + +* [Phoenix 6 swerve with ChoreoLib](https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/SwerveWithChoreo/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java#L196-L215) +* [Phoenix 6 swerve with PathPlannerLib](https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java#L182-L200) diff --git a/docs/choreolib/auto-factory.md b/docs/choreolib/auto-factory.md new file mode 100644 index 000000000..e5fc222ee --- /dev/null +++ b/docs/choreolib/auto-factory.md @@ -0,0 +1,110 @@ + +# AutoFactory + +ChoreoLib provides the `AutoFactory` class as a higher level API to make it easier to create complex auto routines inside your robot code. + +## Basic Usage + +You can set up the `AutoFactory` by calling `Choreo.createAutoFactory`. +The `AutoFactory` is recommended to be created in the robot class constructor instead of the drive subsystem. + +```java +// The most basic usage of the AutoFactory +class Robot extends TimedRobot { + /** A swerve drive subsystem */ + private final Drive drive = ...; + private final AutoFactory autoFactory; + + public Robot() { + autoFactory = Choreo.createAutoFactory( + drive, // The drive subsystem + drive::getPose, // A function that returns the current robot pose + drive::choreoController, // The controller for the drive subsystem + this::isRedAlliance, // A function that returns true if the robot is on the red alliance + new AutoBindings() // An empty `AutoBindings` object, you can learn more below + ); + } + + public void autonomousInit() { + // Running just the movement of a specific trajectory + autoFactory.trajectoryCommand("myTrajectory").schedule(); + } + + private boolean isRedAlliance() { + return DriverStation.getAlliance().orElseGet(() -> Alliance.Blue).equals(Alliance.Red); + } +} +``` + +You can learn more about `drive::choreoController` in the [AutoController](./auto-controller.md) documentation. + + +## Using AutoChooser, AutoRoutine, and AutoTrajectory + +The `AutoFactory` can create `AutoRoutine` and `AutoTrajectory` objects that can be used to create complex auto routines. +The `AutoChooser` provides a simple API to structure your auto routine creation in the most performant way. +`AutoChooser` only creates the `AutoRoutine` on dashboard chooser selection, which prevents loading all routines on boot, +increasing startup times and preventing the routines from being generated on auto start causing a delay. + +For a more advanced example of creating `AutoRoutine` see the [AutoRoutine](./auto-routines.md) documentation. + +```java +// Picking up where the last example left off +public Robot extends TimedRobot { + ... //fields from previous example + /** A subsystem that controls the intake */ + private final Intake intake = ...; + /** A subsystem that controls the shooter */ + private final Shooter shooter = ...; + private final AutoChooser autoChooser; + + public Robot() { + ... //code from previous example + autoChooser = new AutoChooser(autoFactory, ""); + autoChooser.addRoutine("twoPieceAuto", this::twoPieceAuto); + } + + // this would normally be in a separate file + private AutoRoutine twoPieceAuto(AutoFactory factory) { + final AutoRoutine routine = factory.newRoutine("twoPieceAuto"); + + final AutoTrajectory trajectory = factory.trajectory("twoPieceAuto", routine); + + routine.running() + .onTrue( + drive.resetOdometry( + trajectory.getInitialPose() + .orElseGet( + () -> { + routine.kill(); + return new Pose2d(); + })) + .andThen(trajectory.cmd()) + .withName("twoPieceAuto entry point")); + + trajectory.atTime("intake").onTrue(intake.extend()); + trajectory.atTime("shoot").onTrue(shooter.launch()); + + return routine; + } + + public void autonomousInit() { + autoChooser.getSelectedAutoRoutine().schedule(); + } +} +``` + +## AutoBindings + +`AutoBindings` is used to bind event markers in trajectories made by the `AutoFactory` to commands. +This is useful if you have simpler actions that you want to trigger at specific points in a trajectory +without much thought from the user code side. + +```java +AutoBindings makeAutoBindings() { + AutoBindings bindings = new AutoBindings(); + bindings.bind("intake", intake::extend); + bindings.bind("shoot", shooter::launch); + return bindings; +} +``` diff --git a/docs/choreolib/auto-routines.md b/docs/choreolib/auto-routines.md index 07a0fc865..ba635cbff 100644 --- a/docs/choreolib/auto-routines.md +++ b/docs/choreolib/auto-routines.md @@ -1,44 +1,50 @@ # Auto Routines -ChoreoLib provides the `AutoFactory` class as higher level API to make it easier to create competitive and complex auto routines inside your robot code. - ## Triggers vs Composition Composition is how most teams currently architect their auto routines. -You start with 1 `SequentialCommandGroup` and add commands to it. +You start with one `SequentialCommandGroup` and add commands to it. This works for many use cases but can get unwieldy when you have branches, -want your subsystem default command to run during an auto -or want to have concurrent command groups running independently that might handoff `Subsystems` to each other. +want your subsystem default command to run during an auto, or want to have +concurrent command groups running independently that might hand off `Subsystems` to each other. Triggers aim to solve these problems by providing a way to define a control flow based on reactions to state -that don't allocate `Subsystems` until they are needed. Triggers also allow a non-linear control flow with -branching supported in a first class manner. +that don't require `Subsystems` until they are needed. + +Triggers and composition can be used together to create complex auto routines. +Both paradigms also support branching in their own way. + +!!! warning + Triggers can have "hygiene" issues if not used correctly. + Triggers that are polled by the `CommandScheduler` should never + be on the left-hand side of a `and`/`or` method call. + This will leak the trigger outside of the auto routine and can cause + unexpected behavior. For example, this can cause a check intended for intaking + a game piece during an auto to also be checked and reacted to during teleop. ## Monolithic vs Segmented Trajectories -Monolithic trajectories are a single trajectory that is run from start to finish without stopping. -This has the advantage of being simple to create and understand but can be limiting in complex autos that may require -branching. Making a new auto with this method can be time consuming as you have to create a completely new trajectory -for each new auto and are likely repeating yourself. +Monolithic trajectories are based around a single trajectory that is run from start to finish without stopping. +This strategy is simpler to create and understand but can be limiting in complex autos that may require +branching. Making a new auto with this method can be time-consuming as you have to create a completely new trajectory +for each new auto and likely repeat yourself for each trajectory. -Segmented trajectories are a series of smaller trajectories that have defined handoff positions to smoothly transition -between them. This allows for more complex autos to be created by reusing smaller pieces of trajectories and combining -them in different ways. This can be more complex to create but can be more powerful and flexible in the long run. If every -command is named aswell it is easier to debug and understand what is happening in the auto opposed to a monolithic command group. +Segmented trajectories break down complex autonomous routines into smaller, reusable trajectories with defined transition points between them. This method allows for more complex autos to be created by reusing smaller pieces of trajectories and combining +them in different ways. This can be more complex to create but can be more powerful and flexible in the long run. In addition, if each command is clearly named, it is easier to debug and understand what is happening in the auto as opposed to a monolithic command group. ## Example Preamble For the following examples we will be working on a 2024 robot similar to [1678 2024](https://www.statbotics.io/team/1678/2024). -There are already some helper methods in scope to allow us to use commands easier. +There are already some helper methods in scope that return commands for utility: - `intake` - Extends the intake, ends when a note is acquired and automatically retracts when the command ends. Uses the `Intake` `Subsystem`. - `retractIntake` - Retracts the intake. Uses the `Intake` `Subsystem`. -- `shootIfGp` - Shoots the note if the robot has one. +- `shootIfNoteOwned` - Shoots the note if the robot owns a note. Uses the `Shooter` `Subsystem`. -- `aimFor(Pose2d pose)` - Aims the shooter for a specific position, also keeps the wheels spunup. +- `aimFor(Pose2d pose)` - Aims the shooter for a specific position, also keeps the wheels spun up. Uses the `Shooter` `Subsystem`. - `spinnup` - Spins up the shooter wheels. Uses the `Shooter` `Subsystem`. @@ -49,18 +55,18 @@ Uses the `Drive` `Subsystem`. - `autoAimAndShoot` - Aims the shooter and rotates the robot to the correct angle to shoot and then shoots. Uses the `Shooter` and `Drive` `Subsystem`. -There is also a method to make a trigger that represents if the robot owns a note. +There are also two methods that return a trigger that represents if the robot owns a note: -- `yeGp(AutoRoutine routine)` - Returns a trigger that is true if the robot owns a note. -- `noGp(AutoRoutine routine)` - Returns a trigger that is true if the robot does not own a note. +- `noteOwned(AutoRoutine routine)` - Returns a trigger that is true if the robot owns a note. +- `noteNotOwned(AutoRoutine routine)` - Returns a trigger that is true if the robot does not own a note. -There is also a helpful trigger that represents if subsystems are available to be scheduled on. +There is also a trigger that represents if subsystems are available to be scheduled on: - `subsystemsAvailable(AutoRoutine routine, Set subsystems)` - Returns a trigger that is true if the subsystems are available to be scheduled on. -Also assume a `import static edu.wpi.first.wpilibj2.command.Commands.*` is in scope. +These examples also assume that a `import static edu.wpi.first.wpilibj2.command.Commands.*` is in scope. -## Creating an auto routine with triggers and a segmented trajectory +### Creating an auto routine with triggers and a segmented trajectory ```java public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) { @@ -110,21 +116,21 @@ public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) { .and(routine.running()).onTrue(spinnup()); // shoots the note if the robot has it, then runs the trajectory to the first middle note - ampToC1.done().onTrue(shootIfGp()).onTrue( - c1ToM1.cmd().beforeStarting(waitUntil(noGp(routine)))); + ampToC1.done().onTrue(shootIfNoteOwned()).onTrue( + c1ToM1.cmd().beforeStarting(waitUntil(noteNotOwned(routine)))); // extends the intake while traveling towards the first middle note // if the robot has the note, it goes back to shoot it, // otherwise it goes to the next middle note c1ToM1.atTime("intake").onTrue(intake()); - c1ToM1.done().and(yeGp(routine)).onTrue(m1ToS1.cmd()); - c1ToM1.done().and(noGp(routine)).onTrue(m1ToM2.cmd()); + c1ToM1.done().and(noteOwned(routine)).onTrue(m1ToS1.cmd()); + c1ToM1.done().and(noteNotOwned(routine)).onTrue(m1ToM2.cmd()); // aims the shooter while traveling to shoot m1ToS1.active().whileTrue(aimFor(m1ToS1.getFinalPose().orElseGet(Pose2d::new))); - m1ToS1.done().onTrue(shootIfGp()); + m1ToS1.done().onTrue(shootIfNoteOwned()); m1ToS1.done().onTrue(m1ToM2.cmd() - .beforeStarting(waitUntil(noGp(routine)))); + .beforeStarting(waitUntil(noteNotOwned(routine)))); // extends the intake while traveling towards the second middle note // go back to shoot no matter what @@ -133,32 +139,32 @@ public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) { // aims the shooter while traveling to shoot m2ToS1.active().whileTrue(aimFor(m2ToS1.getFinalPose().orElseGet(Pose2d::new))); - m2ToS1.done().onTrue(shootIfGp()); + m2ToS1.done().onTrue(shootIfNoteOwned()); m2ToS1.done().onTrue(s1ToC2.cmd() - .beforeStarting(waitUntil(noGp(routine)))); + .beforeStarting(waitUntil(noteNotOwned(routine)))); // extends the intake while traveling towards the second close note // if the robot has the note, it shoots it // otherwise it goes to the third close note s1ToC2.active().whileTrue(intake()); s1ToC2.active().whileTrue(aimFor(s1ToC2.getFinalPose().orElseGet(Pose2d::new))); - s1ToC2.done().onTrue(shootIfGp()); + s1ToC2.done().onTrue(shootIfNoteOwned()); s1ToC2.done().onTrue(c2ToC3.cmd() - .beforeStarting(waitUntil(noGp(routine)))); + .beforeStarting(waitUntil(noteNotOwned(routine)))); // extends the intake while traveling towards the third close note // if the robot has the note, it shoots it c2ToC3.active().whileTrue(intake()); - c2ToC3.done().onTrue(shootIfGp()); + c2ToC3.done().onTrue(shootIfNoteOwned()); return routine; } ``` -## Creating an auto routine with triggers and a monolithic trajectory +### Creating an auto routine with triggers and a monolithic trajectory ```java -public AutoRoutine fivePieceAutoTriggerMono(AutoFactory factory) { +public Command fivePieceAutoTriggerMono(AutoFactory factory) { final AutoRoutine routine = factory.newRoutine("fivePieceAuto"); final AutoTrajectory trajectory = factory.trajectory("fivePieceAuto", routine); @@ -186,7 +192,7 @@ public AutoRoutine fivePieceAutoTriggerMono(AutoFactory factory) { // extends the intake when the intake event marker is reached trajectory.atTime("intake").onTrue(intake()); // shoots the note when the shoot event marker is reached - trajectory.atTime("shoot").onTrue(shootIfGp()); + trajectory.atTime("shoot").onTrue(shootIfNoteOwned()); // aims the shooter when the aim event marker is reached, // the aim command aims based on the next shoot event marker position @@ -199,7 +205,7 @@ public AutoRoutine fivePieceAutoTriggerMono(AutoFactory factory) { } ``` -## Creating an auto routine with composition and a segmented trajectory +### Creating an auto routine with composition and a segmented trajectory ```java @@ -221,37 +227,37 @@ public AutoRoutine fivePieceAutoCompositionSeg(AutoFactory factory) { final Command s1ToC2 = factory.trajectoryCommand("s1ToC2"); final Command c2ToC3 = factory.trajectoryCommand("c2ToC3"); - Pose2d startingPose; + Pose2d startinNoteOwnedose; if (ampToC1.getInitialPose().isPresent()) { - startingPose = ampToC1.getInitialPose().get(); + startinNoteOwnedose = ampToC1.getInitialPose().get(); } else { return none(); } Command ret = sequence( - resetOdometry(startingPose), + resetOdometry(startinNoteOwnedose), autoAimAndShoot(), deadline( ampToC1.cmd(), intake(), aimFor(ampToC1.getFinalPose().orElseGet(Pose2d::new))), - shootIfGp(), + shootIfNoteOwned(), deadline(c1ToM1, waitSeconds(0.35).andThen(intake())), - new ConditionalCommand( - deadline(m1ToS1, aim()).andThen(shootIfGp()), - deadline(m1ToM2, intake()).andThen(deadline(m2ToS1, aim()), shootIfGp()), - yeGp() // if you aren't using the triggers API these wouldn't need a custom routine - ), + either( + deadline(m1ToS1, aim()).andThen(shootIfNoteOwned()), + deadline(m1ToM2, intake()).andThen(deadline(m2ToS1, aim()), shootIfNoteOwned()), + noteOwned() // if you aren't using the triggers API these wouldn't need a custom routine + ), deadline(s1ToC2, intake(), aim()), - shootIfGp(), + shootIfNoteOwned(), deadline(c2ToC3, intake(), spinnup()), - shootIfGp()) - .withName("fivePieceAuto"); + shootIfNoteOwned() + ).withName("fivePieceAuto"); return factory.commandAsAutoRoutine(ret); } ``` -## Creating an auto routine with composition and a monolithic trajectory +### Creating an auto routine with composition and a monolithic trajectory ```java -// Don't do this +// This is not recommended for complex autos ``` diff --git a/docs/choreolib/basic-usage.md b/docs/choreolib/basic-usage.md index d326071c5..00995b103 100644 --- a/docs/choreolib/basic-usage.md +++ b/docs/choreolib/basic-usage.md @@ -1,44 +1,57 @@ -# Basic Usage +# Getting Started +## Auto Factory API +!!! tip + This API is recommended for most teams as it provides a more user-friendly, batteries included experience. +The `AutoFactory` class provides a high level API that simplifies the creation of complex autonomous routines in your robot code. Read [the documentation page](./auto-factory.md) for more information. + +## Raw Trajectory API Teams that want to use their own path following structure can load trajectories directly with the following code. -``` { .java .select } -import java.util.Optional; - -import choreo.Choreo; -import choreo.trajectory.ChoreoTrajectory; -import choreo.trajectory.SwerveSample; - -// Loading a trajectory from a file, returns an optional if the file does not exist or is invalid -var trajectory = Choreo.loadTrajectory("myTrajectory"); -if (trajectory.isPresent()) { - // Do something with the trajectory - drive.followTrajectory(trajectory.get()).schedule(); -} else { - // If the trajectory is not found, ChoreoLib already prints to DriverStation -} -``` -``` { .py .select } -import choreo - -# Loading a trajectory from a file, throwing ValueError if the file does not exist or is invalid -try: - trajectory = choreo.load_swerve_trajectory("myTrajectory") - - # Do something with the trajectory - drive.followTrajectory(trajectory).schedule() -except ValueError: - # If the trajectory is not found, ChoreoLib already prints to DriverStation - pass -``` -``` { .cpp .select } -#include - -// Loading a trajectory from a file, returns an optional if the file does not exist or is invalid -if (auto trajectory = choreo::LoadTrajectory("myTrajectory")) { - // Do something with the trajectory - drive.followTrajectory(trajectory.value()).schedule(); -} else { - // If the trajectory is not found, ChoreoLib already prints to DriverStation -} -``` +=== "Java" + + ```java + import java.util.Optional; + + import choreo.Choreo; + import choreo.trajectory.ChoreoTrajectory; + import choreo.trajectory.SwerveSample; + + // Loading a trajectory from a file, returns an optional if the file does not exist or is invalid + var trajectory = Choreo.loadTrajectory("myTrajectory"); + if (trajectory.isPresent()) { + // Do something with the trajectory + drive.followTrajectory(trajectory.get()).schedule(); + } else { + // If the trajectory is not found, ChoreoLib already prints to DriverStation + } + ``` + +=== "Python" + ```py + import choreo + + # Loading a trajectory from a file, throwing ValueError if the file does not exist or is invalid + try: + trajectory = choreo.load_swerve_trajectory("myTrajectory") + + # Do something with the trajectory + drive.followTrajectory(trajectory).schedule() + except ValueError: + # If the trajectory is not found, ChoreoLib already prints to DriverStation + pass + ``` + +=== "C++" + + ```cpp + #include + + // Loading a trajectory from a file, returns an optional if the file does not exist or is invalid + if (auto trajectory = choreo::LoadTrajectory("myTrajectory")) { + // Do something with the trajectory + drive.followTrajectory(trajectory.value()).schedule(); + } else { + // If the trajectory is not found, ChoreoLib already prints to DriverStation + } + ``` diff --git a/docs/choreolib/installation.md b/docs/choreolib/installation.md index 4e0661071..ec6755add 100644 --- a/docs/choreolib/installation.md +++ b/docs/choreolib/installation.md @@ -1,4 +1,6 @@ -Install ChoreoLib to your robot project by copy pasting the following link into the vendor library installation dialog: +# Installation Methods + +## Vendor Dependencies ``` # Release @@ -8,7 +10,7 @@ https://sleipnirgroup.github.io/ChoreoLib/dep/ChoreoLib.json https://sleipnirgroup.github.io/ChoreoLib/dep/ChoreoLibBeta.json ``` -The installation method is the same as CTRE, PathPlanner, and more. Read more on Vendor Dependencies and their installation (VSCode —> install new library (online)) [here](https://docs.wpilib.org/en/stable/docs/software/vscode-overview/3rd-party-libraries.html#installing-libraries). +The installation method is the same as CTRE, PathPlanner, and more. Read more on Vendor Dependencies and their installation (VSCode → install new library (online)) [here](https://docs.wpilib.org/en/stable/docs/software/vscode-overview/3rd-party-libraries.html#installing-libraries). ## Building Manually @@ -21,4 +23,4 @@ The built library will be located in the respective operating system's m2 folder - Unix/Mac OS X - `~/.m2/repository` - Windows – `C:\Users\{your-username}\.m2\repository` -To use a custom built version you'll need to update your ChoreoLib.json file to point to the local repository and version. +To use a custom-built version you'll need to update your ChoreoLib.json file to point to the local repository and version. diff --git a/mkdocs.yml b/mkdocs.yml index 924cae973..bbd4f767b 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -73,6 +73,11 @@ markdown_extensions: - pymdownx.inlinehilite - pymdownx.snippets - pymdownx.superfences + - pymdownx.tabbed: + alternate_style: true + slugify: !!python/object/apply:pymdownx.slugs.slugify + kwds: + case: lower extra_javascript: - js/mathjax.js @@ -94,8 +99,10 @@ nav: - Estimating Moment of Inertia: usage/estimating-moi.md - ChoreoLib: - Installation: choreolib/installation.md - - Basic Usage: choreolib/basic-usage.md + - Getting Started: choreolib/basic-usage.md + - Auto Factory: choreolib/auto-factory.md - Auto Routines: choreolib/auto-routines.md + - Auto Controller: choreolib/auto-controller.md - Java API: api/choreolib/java/index.html - C++ API: api/choreolib/cpp/index.html - TrajoptLib (Internal API):