auton examples

comp/src/main/java/org/team100/frc2026/Robot.java

@@ -1,6 +1,6 @@
 package org.team100.frc2026;
 
-import org.team100.frc2026.robot.AllAutons;
+import org.team100.frc2026.auton.AllAutons;
 import org.team100.frc2026.robot.Binder;
 import org.team100.frc2026.robot.Machinery;
 import org.team100.frc2026.robot.Prewarmer;
@@ -10,6 +10,8 @@
 import org.team100.lib.experiments.Experiment;
 import org.team100.lib.experiments.Experiments;
 import org.team100.lib.framework.TimedRobot100;
+import org.team100.lib.indicator.Alerts;
+import org.team100.lib.indicator.AutonAlerts;
 import org.team100.lib.logging.RobotLog;
 import org.team100.lib.util.Banner;
 
@@ -28,6 +30,8 @@ public class Robot extends TimedRobot100 {
 
     private final RobotLog m_robotLog;
     private final Machinery m_machinery;
+    private final Alerts m_alerts;
+    private final AutonAlerts m_autonAlerts;
     private final AllAutons m_allAutons;
     private final Binder m_binder;
 
@@ -53,10 +57,16 @@ public Robot() {
         m_robotLog = new RobotLog();
 
         m_machinery = new Machinery();
-        m_allAutons = new AllAutons(m_machinery);
         m_binder = new Binder(m_machinery);
         m_binder.bind();
 
+        m_allAutons = new AllAutons(m_machinery, m_binder.m_holonomicController);
+        m_alerts = new Alerts();
+        m_autonAlerts = new AutonAlerts(
+                m_allAutons::getAnnotated,
+                m_alerts,
+                m_machinery::resetPose);
+
         Prewarmer.init(m_machinery);
     }
 
@@ -77,6 +87,12 @@ public void robotPeriodic() {
         }
     }
 
+    /** Check the auton configuration when disabled. */
+    @Override
+    public void disabledPeriodic() {
+        m_autonAlerts.run();
+    }
+
     //////////////////////////////////////////////////////////////////////
     //
     // INITIALIZERS, DO NOT CHANGE THESE
@@ -127,10 +143,6 @@ public void testInit() {
     public void simulationPeriodic() {
     }
 
-    @Override
-    public void disabledPeriodic() {
-    }
-
     @Override
     public void autonomousPeriodic() {
     }

comp/src/main/java/org/team100/frc2026/auton/AllAutons.java

@@ -0,0 +1,55 @@
+package org.team100.frc2026.auton;
+
+import org.team100.frc2026.robot.Machinery;
+import org.team100.lib.config.AnnotatedCommand;
+import org.team100.lib.config.AutonChooser;
+import org.team100.lib.controller.se2.ControllerSE2;
+import org.team100.lib.logging.LoggerFactory;
+import org.team100.lib.logging.Logging;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * Populates the Auton chooser with all available autons.
+ * 
+ * It's a good idea to instantiate them all here, even if you're not using them
+ * all, even if they're just in development, so they don't rot.
+ * 
+ * In 2026, the field is rotationally symmetric, so there's no need to fill out
+ * the "Alliance" part of the annotated command.
+ */
+public class AllAutons {
+    private final AutonChooser m_autonChooser;
+
+    public AllAutons(Machinery machinery, ControllerSE2 controller) {
+        m_autonChooser = new AutonChooser();
+        LoggerFactory log = Logging.instance().rootLogger.name("Auton");
+        m_autonChooser.add(new DoNothing());
+        m_autonChooser.add(new RightTrenchLeave(
+                log,
+                machinery.m_swerveKinodynamics,
+                controller,
+                machinery));
+        m_autonChooser.add(new LeftSequenceExample(
+                log,
+                machinery.m_swerveKinodynamics,
+                controller,
+                machinery));
+    }
+
+    public Command get() {
+        AnnotatedCommand annotatedCommand = getAnnotated();
+        if (annotatedCommand == null)
+            return null;
+        return annotatedCommand.command();
+    }
+
+    public AnnotatedCommand getAnnotated() {
+        return m_autonChooser.get();
+    }
+
+    public void close() {
+        m_autonChooser.close();
+    }
+
+}

comp/src/main/java/org/team100/frc2026/auton/DoNothing.java

@@ -0,0 +1,26 @@
+package org.team100.frc2026.auton;
+
+import org.team100.lib.config.AnnotatedCommand;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+
+/** An auton that does nothing at all. */
+public class DoNothing implements AnnotatedCommand {
+
+    @Override
+    public String name() {
+        return "Do Nothing";
+    }
+
+    @Override
+    public Command command() {
+        return Commands.idle().withName("Nothing from right bump");
+    }
+
+    @Override
+    public Pose2d start() {
+        return StartingPositions.RIGHT_BUMP;
+    }
+}

comp/src/main/java/org/team100/frc2026/auton/LeftSequenceExample.java

@@ -0,0 +1,109 @@
+package org.team100.frc2026.auton;
+
+import static edu.wpi.first.wpilibj2.command.Commands.sequence;
+import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds;
+
+import java.util.List;
+
+import org.team100.frc2026.robot.Machinery;
+import org.team100.lib.config.AnnotatedCommand;
+import org.team100.lib.controller.se2.ControllerSE2;
+import org.team100.lib.geometry.DirectionSE2;
+import org.team100.lib.geometry.WaypointSE2;
+import org.team100.lib.logging.LoggerFactory;
+import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction;
+import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
+import org.team100.lib.trajectory.TrajectorySE2;
+import org.team100.lib.trajectory.TrajectorySE2Factory;
+import org.team100.lib.trajectory.TrajectorySE2Planner;
+import org.team100.lib.trajectory.constraint.TimingConstraint;
+import org.team100.lib.trajectory.constraint.TimingConstraintFactory;
+import org.team100.lib.trajectory.path.PathSE2Factory;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** An example of a simple sequence */
+public class LeftSequenceExample implements AnnotatedCommand {
+    private final LoggerFactory log;
+    private final ControllerSE2 controller;
+    private final Machinery machinery;
+    private final List<TimingConstraint> constraints;
+    private final TrajectorySE2Factory trajectoryFactory;
+    private final PathSE2Factory pathFactory;
+    private final TrajectorySE2Planner planner;
+
+    public LeftSequenceExample(
+            LoggerFactory parent,
+            SwerveKinodynamics kinodynamics,
+            ControllerSE2 controller,
+            Machinery machinery) {
+        log = parent.name("RightTrenchLeave");
+        this.controller = controller;
+        this.machinery = machinery;
+        constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this));
+        trajectoryFactory = new TrajectorySE2Factory(constraints);
+        pathFactory = new PathSE2Factory();
+        planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory);
+    }
+
+    @Override
+    public String name() {
+        return "Left Sequence Example";
+    }
+
+    // go to the middle while spinning 90 degrees
+    TrajectorySE2 t1(Pose2d startingPose) {
+        List<WaypointSE2> waypoints = List.of(
+                new WaypointSE2(
+                        startingPose,
+                        new DirectionSE2(-1, 0, 0),
+                        1),
+                new WaypointSE2(
+                        new Pose2d(2, 4, Rotation2d.kCW_Pi_2),
+                        new DirectionSE2(0, -1, 0),
+                        1));
+        return planner.restToRest(waypoints);
+    }
+
+    // go back where we started.
+    TrajectorySE2 t2(Pose2d startingPose) {
+        List<WaypointSE2> waypoints = List.of(
+                new WaypointSE2(
+                        startingPose,
+                        new DirectionSE2(0, 1, 0),
+                        1),
+                new WaypointSE2(
+                        StartingPositions.LEFT_TRENCH,
+                        new DirectionSE2(1, 0, 0),
+                        1));
+        return planner.restToRest(waypoints);
+    }
+
+    @Override
+    public Command command() {
+        DriveWithTrajectoryFunction n1 = new DriveWithTrajectoryFunction(
+                log,
+                machinery.m_drive,
+                controller,
+                machinery.m_trajectoryViz,
+                this::t1);
+        DriveWithTrajectoryFunction n2 = new DriveWithTrajectoryFunction(
+                log,
+                machinery.m_drive,
+                controller,
+                machinery.m_trajectoryViz,
+                this::t2);
+        return sequence(
+                n1.until(n1::isDone),
+                waitSeconds(1),
+                n2.until(n2::isDone));
+    }
+
+    @Override
+    public Pose2d start() {
+        return StartingPositions.LEFT_TRENCH;
+    }
+
+}

comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java

@@ -0,0 +1,86 @@
+package org.team100.frc2026.auton;
+
+import java.util.List;
+
+import org.team100.frc2026.robot.Machinery;
+import org.team100.lib.config.AnnotatedCommand;
+import org.team100.lib.controller.se2.ControllerSE2;
+import org.team100.lib.geometry.DirectionSE2;
+import org.team100.lib.geometry.WaypointSE2;
+import org.team100.lib.logging.LoggerFactory;
+import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction;
+import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
+import org.team100.lib.trajectory.TrajectorySE2;
+import org.team100.lib.trajectory.TrajectorySE2Factory;
+import org.team100.lib.trajectory.TrajectorySE2Planner;
+import org.team100.lib.trajectory.constraint.TimingConstraint;
+import org.team100.lib.trajectory.constraint.TimingConstraintFactory;
+import org.team100.lib.trajectory.path.PathSE2Factory;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * Drives a short distance from the right trench starting point.
+ * 
+ * There are no points for leaving the starting line, as there sometimes has
+ * been in other years. This is just a demo.
+ */
+public class RightTrenchLeave implements AnnotatedCommand {
+    private final LoggerFactory log;
+    private final ControllerSE2 controller;
+    private final Machinery machinery;
+    private final List<TimingConstraint> constraints;
+    private final TrajectorySE2Factory trajectoryFactory;
+    private final PathSE2Factory pathFactory;
+    private final TrajectorySE2Planner planner;
+
+    public RightTrenchLeave(
+            LoggerFactory parent,
+            SwerveKinodynamics kinodynamics,
+            ControllerSE2 controller,
+            Machinery machinery) {
+        log = parent.name("RightTrenchLeave");
+        this.controller = controller;
+        this.machinery = machinery;
+        constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this));
+        trajectoryFactory = new TrajectorySE2Factory(constraints);
+        pathFactory = new PathSE2Factory();
+        planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory);
+    }
+
+    @Override
+    public String name() {
+        return "Right Trench Leave";
+    }
+
+    TrajectorySE2 trajectory(Pose2d startingPose) {
+        List<WaypointSE2> waypoints = List.of(
+                new WaypointSE2(
+                        startingPose,
+                        new DirectionSE2(-1, 0, 0),
+                        1),
+                new WaypointSE2(
+                        new Pose2d(2, 4, Rotation2d.kZero),
+                        new DirectionSE2(0, 1, 0),
+                        1));
+        return planner.restToRest(waypoints);
+    }
+
+    @Override
+    public Command command() {
+        DriveWithTrajectoryFunction navigator = new DriveWithTrajectoryFunction(
+                log,
+                machinery.m_drive,
+                controller,
+                machinery.m_trajectoryViz,
+                this::trajectory);
+        return navigator.until(navigator::isDone);
+    }
+
+    @Override
+    public Pose2d start() {
+        return StartingPositions.RIGHT_TRENCH;
+    }
+}

comp/src/main/java/org/team100/frc2026/auton/StartingPositions.java

@@ -0,0 +1,21 @@
+package org.team100.frc2026.auton;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/**
+ * These measurements are guesses.
+ * 
+ * G303: Starting positions must overlap the starting line and not contact the
+ * bump.
+ */
+public class StartingPositions {
+    /** Barely overlaps the tape */
+    private static final double X = 4.0;
+    public static final Pose2d LEFT_TRENCH = new Pose2d(X, 7.4, Rotation2d.kZero);
+    public static final Pose2d LEFT_BUMP = new Pose2d(X, 5.5, Rotation2d.kZero);
+    public static final Pose2d CENTER = new Pose2d(X, 4.0, Rotation2d.kZero);
+    public static final Pose2d RIGHT_BUMP = new Pose2d(X, 2.5, Rotation2d.kZero);
+    public static final Pose2d RIGHT_TRENCH = new Pose2d(X, 0.6, Rotation2d.kZero);
+
+}