Ensure alliance is initialized in examples

SleipnirGroup · Nov 28, 2024 · ec9b399 · ec9b399
1 parent 20ff02e
commit ec9b399
Showing 1 changed file with 38 additions and 37 deletions.
diff --git a/docs/choreolib/auto-routines.md b/docs/choreolib/auto-routines.md
@@ -50,8 +50,9 @@ Uses the `Shooter` `Subsystem`.
 Uses the `Shooter` `Subsystem`.
 - `aim` - Aims the shooter based on the current odometry position.
 Uses the `Shooter` `Subsystem`.
-- `resetOdometry(Pose2d pose)` - Resets the odometry to a specific position.
-Uses the `Drive` `Subsystem`.
+- `resetOdometry(Supplier<Pose2d> pose)` - Resets the odometry to a specific position.
+Uses the `Drive` `Subsystem`, and accepts a `Supplier` to ensure that the initial pose is fetched 
+after the robot code knows the current alliance.
 - `autoAimAndShoot` - Aims the shooter and rotates the robot to the correct angle to shoot and then shoots.
 Uses the `Shooter` and `Drive` `Subsystem`.
 
@@ -71,6 +72,7 @@ These examples also assume that a `import static edu.wpi.first.wpilibj2.command.
 ```java
 public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) {
   final AutoRoutine routine = factory.newRoutine("fivePieceAuto");
+  final Alert noStartingPoseErr = new Alert("Error: 5 piece auto has no starting pose", AlertType.kError);
 
   // This uses segments that all have predefined handoff points.
   // These handoff points follow a naming convention
@@ -89,27 +91,27 @@ public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) {
   final AutoTrajectory s1ToC2 = factory.trajectory("s1ToC2", routine);
   final AutoTrajectory c2ToC3 = factory.trajectory("c2ToC3", routine);
 
-  final Optional<Pose2d> initialPose = ampToC1.getInitialPose();
-  if (initialPose.isEmpty()) {
-      new Alert("Error: 5 piece auto has no starting pose", AlertType.kError)
-          .set(true);
-      return factory.voidRoutine();
-  }
-
   // entry point for the auto
   // resets the odometry to the starting position,
   // then shoots the starting note,
   // then runs the trajectory to the first close note while extending the intake
   routine.running()
       .onTrue(
-          resetOdometry(initialPose.get())
-              .andThen(
-                  autoAimAndShoot(),
-                  race(
-                      intake(),
-                      ampToC1.cmd(),
-                      aimFor(ampToC1.getFinalPose().orElseGet(Pose2d::new))))
-              .withName("fivePieceAuto entry point"));
+          resetOdometry(() -> {
+              final Optional<Pose2d> initialPose = ampToC1.getInitialPose();
+              if (initialPose.isPresent()) return initialPose.get();
+              noStartingPoseErr.set(true);
+              routine.kill();
+              return new Pose2d();
+          }).andThen(
+              autoAimAndShoot(),
+              race(
+                  intake(),
+                  ampToC1.cmd(),
+                  aimFor(ampToC1.getFinalPose().orElseGet(Pose2d::new))
+              )
+          ).withName("fivePieceAuto entry point")
+      );
 
   // spinnup the shooter while no other command is using the shooter
   subsystemsAvailable(routine, spinnup().getRequirements())
@@ -166,25 +168,25 @@ public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) {
 ```java
 public Command fivePieceAutoTriggerMono(AutoFactory factory) {
   final AutoRoutine routine = factory.newRoutine("fivePieceAuto");
-
+  final Alert noStartingPoseErr = new Alert("Error: 5 piece auto has no starting pose", AlertType.kError);
   final AutoTrajectory trajectory = factory.trajectory("fivePieceAuto", routine);
-
-  final Optional<Pose2d> initialPose = trajectory.getInitialPose();
-  if (initialPose.isEmpty()) {
-      new Alert("Error: FivePieceAuto has no starting pose", AlertType.kError)
-          .set(true);
-      return factory.voidRoutine();
-  }
 
   // entry point for the auto
   // resets the odometry to the starting position,
   // then shoots the starting note,
   // then runs the trajectory to the first close note while extending the intake
   routine.running()
       .onTrue(
-          resetOdometry(initialPose.get())
-              .andThen(autoAimAndShoot(), trajectory.cmd())
-              .withName("fivePieceAuto entry point"));
+          resetOdometry(() -> {
+              final Optional<Pose2d> initialPose = ampToC1.getInitialPose();
+              if (initialPose.isPresent()) return initialPose.get();
+              noStartingPoseErr.set(true);
+              routine.kill();
+              return new Pose2d();
+          })
+          .andThen(autoAimAndShoot(), trajectory.cmd())
+          .withName("fivePieceAuto entry point")
+      );
 
   // spinnup the shooter while no other command is running
   subsystemsAvailable(routine, spinnup().getRequirements())
@@ -218,7 +220,6 @@ public AutoRoutine fivePieceAutoCompositionSeg(AutoFactory factory) {
   // S1, S2, S3: 3 arbitrary shooting positions that are near the stage, S1 having the greatest y
   // value
   // AMP, SUB, SRC: The 3 starting positions
-
   // Try to load all the trajectories we need
   final AutoTrajectory ampToC1 = factory.trajectory("ampToC1", factory.voidLoop());
   final Command c1ToM1 = factory.trajectoryCommand("c1ToM1");
@@ -227,16 +228,16 @@ public AutoRoutine fivePieceAutoCompositionSeg(AutoFactory factory) {
   final Command m2ToS1 = factory.trajectoryCommand("m2ToS2");
   final Command s1ToC2 = factory.trajectoryCommand("s1ToC2");
   final Command c2ToC3 = factory.trajectoryCommand("c2ToC3");
-
-  final Optional<Pose2d> initialPose = ampToC1.getInitialPose();
-  if (initialPose.isEmpty()) {
-      new Alert("Error: 5 piece auto has no starting pose", AlertType.kError)
-          .set(true);
-      return factory.voidRoutine();
-  }
+  final Alert noStartingPoseErr = new Alert("Error: 5 piece auto has no starting pose", AlertType.kError);
 
   Command ret = sequence(
-          resetOdometry(initialPose.get()),
+          resetOdometry(() -> {
+              final Optional<Pose2d> initialPose = ampToC1.getInitialPose();
+              if (initialPose.isPresent()) return initialPose.get();
+              noStartingPoseErr.set(true);
+              routine.kill();
+              return new Pose2d();
+          }),
           autoAimAndShoot(),
           deadline(
               ampToC1.cmd(), intake(), aimFor(ampToC1.getFinalPose().orElseGet(Pose2d::new))),