From ec9b399ee078da9a4ad2d934c7245b0dd05d4db5 Mon Sep 17 00:00:00 2001 From: Daniel Chen Date: Thu, 28 Nov 2024 15:27:16 -0500 Subject: [PATCH] Ensure alliance is initialized in examples --- docs/choreolib/auto-routines.md | 75 +++++++++++++++++---------------- 1 file changed, 38 insertions(+), 37 deletions(-) diff --git a/docs/choreolib/auto-routines.md b/docs/choreolib/auto-routines.md index 3046faeb4..9179c1902 100644 --- 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 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 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 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,15 +168,8 @@ 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 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, @@ -182,9 +177,16 @@ public Command fivePieceAutoTriggerMono(AutoFactory factory) { // 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 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 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 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))),