Dynamic Autonomous
Build an Autonomous routine that allows drivers to select paths on the fly using Gamepads and SequentialCommandGroups.
By the end you can
- Understand the DynamicAuto structure used in the team's repository.
- Learn how to use dynamic path builders (moveTo and curveTo).
- Combine PedroPathing movements with Subsystem commands using SequentialCommandGroup.
Most FTC teams write a completely separate Java class for every single Autonomous path (e.g., RedNearAuto.java, BlueFarAuto.java, RedNearNoPark.java). If you want to change how the robot shoots a ball, you have to edit 8 different files!
Our team uses a Dynamic Autonomous architecture. We write one Java class. During the init() phase, the driver uses the gamepad to answer questions: "Are we Red or Blue?", "Are we Near or Far?", "Do you want to park or not?". The code dynamically stitches together the required paths into a massive SequentialCommandGroup!
The Init Loop: Selecting Paths
How does the robot know what we want it to do? During init(), we run a while (opModeInInit()) loop. Inside this loop, we read the Gamepad inputs and add them to a list called pathSequence.
// We create a List to hold the steps we want to take
private final List<AutoPathPositions> pathSequence = new ArrayList<>();
// ... inside initialize() ...
while (opModeInInit()) {
// Cycle through available positions using the DPAD
if (gamepad2.dpadRightWasPressed()) {
selector = selector.next();
}
if (gamepad2.dpadLeftWasPressed()) {
selector = selector.previous();
}
// A Button: Add current selector to the sequence!
if (gamepad2.aWasPressed()) {
pathSequence.add(selector);
}
// Y Button: Clear sequence if we made a mistake
if (gamepad2.yWasPressed()) {
pathSequence.clear();
}
telemetry.addData("Currently Selecting", selector);
telemetry.addData("Current Route", pathSequence.toString());
telemetry.update();
}This is incredibly powerful! The drive team can look at the field, see where their alliance partner is starting, and instantly build a custom autonomous routine on the fly by pressing the 'A' button.
Building Dynamic Paths
Standard PedroPathing requires you to explicitly define every single path in a giant chain. In a dynamic auto, we don't know the full chain ahead of time!
Instead, we use helper methods that remember the last place the robot ended up (lastPathEndPose), and dynamically build a tiny PathChain from that spot to the next target.
Here is the core logic from our DynamicAuto.java that makes this possible:
// Inside DynamicAuto.java
private Pose lastPathEndPose;
/**
* Helper method to build a straight line from wherever the robot is currently
* sitting, to the target pose.
*/
private Command moveTo(Pose targetPose) {
PathChain pc = follower.pathBuilder()
.addPath(new BezierLine(lastPathEndPose, targetPose))
.setLinearHeadingInterpolation(lastPathEndPose.getHeading(), targetPose.getHeading())
.build();
// Update our tracker so the next movement starts from here!
lastPathEndPose = targetPose;
// Return our custom Wrapper Command!
return new FollowPathCommand(follower, pc, true);
}
/**
* Helper method to build a curve using Bezier control points.
*/
private Command curveTo(Pose targetPose, Pose... controlPoints) {
PathChain pc = follower.pathBuilder()
.addPath(new BezierCurve(combinePoses(lastPathEndPose, targetPose, controlPoints)))
.setLinearHeadingInterpolation(lastPathEndPose.getHeading(), targetPose.getHeading())
.build();
lastPathEndPose = targetPose;
return new FollowPathCommand(follower, pc, true);
}Combining Poses for Curves
When building a BezierCurve, PedroPathing requires an array of control points. Our curveTo() method uses a helper called combinePoses() to sandwich the control points between our start and end positions:
private Pose[] combinePoses(Pose start, Pose end, Pose[] controls) {
// Create a new array large enough to hold start, end, and all controls
Pose[] combined = new Pose[controls.length + 2];
combined[0] = start; // Start goes first
System.arraycopy(controls, 0, combined, 1, controls.length); // Copy controls into the middle
combined[combined.length - 1] = end; // End goes last
return combined;
}Poses.java: Defining the Field
You might have noticed variables like poses.LAUNCH_POSE or poses.FIRST_SPIKE in the code.
Instead of hardcoding numbers like (24, 48, 180) everywhere, professional teams create a separate class (like Poses.java) to define all important locations on the field as constants.
This is especially helpful because the field is usually symmetrical! If you define your Blue Alliance poses, you can write math to automatically flip the Y coordinates and heading for the Red Alliance, saving you from writing two completely different autos!
Stitching Commands Together
Because our moveTo() method returns an FTCLib Command (specifically, the FollowPathCommand we wrote in the last lesson), we can toss these movements into a SequentialCommandGroup right alongside our normal subsystem commands!
Let's look at how the buildDynamicRoutine() method reads the driver's selections and builds the auto:
public Command buildDynamicRoutine() {
// This is the giant container that will hold the entire Auto sequence
SequentialCommandGroup mainRoutine = new SequentialCommandGroup();
// Iterate through whatever selections the driver made during INIT
for (AutoPathPositions pos : pathSequence) {
switch (pos) {
case LAUNCH:
// 1. Move to the launch position
// 2. Fire the balls using our standard Subsystem Command!
mainRoutine.addCommands(
moveTo(poses.LAUNCH_POSE),
new LaunchBallsCommand(robot.launchSubsystem, robot.launchGateSubsystem)
);
break;
case SPIKE_1:
// If we are far away, we have to curve around obstacles to reach Spike 1
if (robotPosition == RobotPosition.FAR) {
mainRoutine.addCommands(
curveTo(poses.FIRST_SPIKE, poses.FAR_SPIKE_1_CONTROL)
);
} else {
// If we are near, we can just drive straight there
mainRoutine.addCommands(
moveTo(poses.FIRST_SPIKE)
);
}
break;
case HUMAN_PLAYER:
// A complex sequence using a ParallelRaceGroup!
// Drive to the human player, turn on the intake, and jiggle back and forth
// until we suck up 3 game pieces!
mainRoutine.addCommands(
moveTo(poses.HUMAN_PLAYER_POSE),
new ParallelRaceGroup(
// 1. The Win Condition: Stop when we have 3 artifacts
new WaitUntilCommand(() -> robot.intakeSubsystem.getArtifactCount() == 3),
// 2. The Timeout: Give up if it takes longer than 1.5 seconds
new WaitCommand(1500),
// 3. The Action: Repeatedly jiggle forward and backward
new RepeatCommand(
new SequentialCommandGroup(
moveTo(poses.HUMAN_JIGGLE_BACKWARD),
moveTo(poses.HUMAN_PLAYER_POSE)
)
)
)
);
break;
}
}
return mainRoutine;
}Advanced FTCLib Tricks
In the HUMAN_PLAYER case above, you see a masterclass in Command-Based programming:
ParallelRaceGroup: This runs multiple commands at the exact same time. However, as soon as any one of the commands finishes, it instantly kills all the other commands!WaitUntilCommand: This command "finishes" the instant the intake sensor detects 3 artifacts. Because it's in a Race Group, this will instantly kill theRepeatCommandthat is making the robot jiggle.WaitCommand(1500): This is a safety timeout. If the human player drops a piece and we never get 3 artifacts, this command finishes after 1.5 seconds, which kills the Race Group and allows the rest of the auto to continue, preventing the robot from being stuck jiggling forever!
Check Your Understanding
Check yourself