Profiled Drive to Point

Phoenix 6 ships a straight-line trip planner called LinearPath. You hand it two pairs of limits, one for driving and one for turning, then ask it the same question every loop. At t seconds into the trip, where should the robot be, and how fast should it be going?

The answer is a LinearPath.State, holding a pose and a velocity. The pose is what PID measures against. The velocity is the feedforward: the speed the plan says the robot should be doing right now.

Plot that planned speed against time and you get a trapezoid, which is where TrapezoidProfile gets its name. The branch drives at 2.5 m/s with 3.0 m/s² of acceleration.

Phase 1

Speed up

Gain 3.0 m/s of speed every second. Reaching 2.5 m/s takes about 0.83 s and covers about 1.04 m.

Phase 2

Cruise

Hold 2.5 m/s for whatever distance is left in the middle. A short trip has no middle.

Phase 3

Slow down

Shed 3.0 m/s every second, timed to arrive at the goal with a planned speed of zero.

Speeding up and slowing down each need about 1.04 m, so a trip shorter than 2.1 m never reaches 2.5 m/s. Those come out as a triangle: speed up, then straight into slowing down. No special case to write.

Three new fields and three rewritten methods, all in the one file. Three imports come with them: com.ctre.phoenix6.Utils, com.ctre.phoenix6.swerve.utility.LinearPath, and org.wpilib.math.trajectory.TrapezoidProfile.

1// The trip planner. First pair of limits: top speed (m/s) and acceleration (m/s²) for
2// driving. Second pair: the same for turning (rad/s, rad/s²).
3// TODO: tune to what your drivetrain can do.
4private final LinearPath path =
5    new LinearPath(
6        new TrapezoidProfile.Constraints(2.5, 3.0),
7        new TrapezoidProfile.Constraints(Math.PI, 2.0 * Math.PI));

Math.PI is half a turn in radians, so the turning limits read as half a turn per second, and one full turn per second squared.

The command builds the plan once, from where the robot was and how fast it was moving when the button went down. That snapshot and a clock reading are the other two fields.

1// Where the robot was, and how fast it was moving, when the command started. The whole
2// trip is planned from this one snapshot.
3private LinearPath.State startState = new LinearPath.State();
4// When the command started. (now - startTime) says how far into the trip we are.
5private double startTime;
6 
7/** Takes the starting snapshot and starts the trip clock. */
8@Override
9protected void initialize() {
10  startState = new LinearPath.State(drivetrain.getPose(), drivetrain.getFieldVelocity());
11  startTime = Utils.getCurrentTimeSeconds();
12  xController.reset();
13  yController.reset();
14  headingController.reset();
15}

A robot already rolling gets a plan that starts from the speed it has, not from a standstill. That is what getFieldVelocity() is doing there.

1/** Runs every robot loop while the command is active. */
2@Override
3protected void execute() {
4  // Ask the plan where we should be, this many seconds into the trip.
5  double t = Utils.getCurrentTimeSeconds() - startTime;
6  LinearPath.State setpoint = path.calculate(t, startState, goal);
7 
8  Pose2d measuredPose = drivetrain.getPose();
9 
10  // The plan's velocity does the driving. Each PID call below adds a small correction that
11  // pulls the measured pose back onto the planned pose.
12  ChassisVelocities feedforward = setpoint.velocity;
13  double vx = feedforward.vx + xController.calculate(measuredPose.getX(), setpoint.pose.getX());
14  double vy = feedforward.vy + yController.calculate(measuredPose.getY(), setpoint.pose.getY());
15  double omega =
16      feedforward.omega
17          + headingController.calculate(
18              measuredPose.getRotation().getRadians(), setpoint.pose.getRotation().getRadians());
19 
20  drivetrain.setControl(driveRequest.withVelocity(new ChassisVelocities(vx, vy, omega)));
21}

The planned velocity goes out as it is. Each controller adds a small correction on top, pulling the measured pose back onto the planned pose. On a perfect floor every correction would be zero and the plan would drive the trip alone.

The finish line is the plan's own clock. isFinished() returns path.isFinished(t), with t the seconds since the command started. No position tolerance anywhere.

The finished file, 120 lines. The GitHub Changes tab is PR #12: 59 lines added, 26 removed.

On 5-DriveToPoint, PID output was the whole commanded velocity, and the error it measured was the distance to the goal. Meters of error, for most of the trip. A kP of 10 paid for that.

Now the error is the gap between the measured pose and setpoint.pose, and that stays in centimeters the whole way. X and Y come down to 3.0, heading to 4.0.

Leave them at 10 and the correction piles onto a plan that was already asking for the right speed. Ten centimeters of drift buys another meter per second. The robot hunts around the path instead of settling onto it.

1. Enable Teleop in the simulator and hold B. The robot eases away, holds a steady speed, and eases off at the end. From the origin that goal is 3.6 m out, past the 2.1 m needed for cruise.
2. Keep holding B after it arrives. The robot stays stopped, because the command finished on its own. On 5-DriveToPoint it would still be pushing.
3. Graph Drivetrain/TranslationSpeedMps for that run. A ramp up, a flat top near 2.5, a ramp down to zero. The flat top is the proof the plan is in charge.
4. Hold A until the robot stops, sending it back to Pose2d.kZero. The speed graph shows the same trapezoid the other way, and Drivetrain/Pose settles near zero. It turns toward 0° while it drives rather than spinning first.

It lunges off in the wrong direction. The startState line is missing from initialize(), so the plan is drawn from the empty new LinearPath.State() the field was given at declaration.

It weaves along the path. The gains are still 10 / 10 / 7. If they are already at 3.0 and 4.0, lower kP further or add a little kD.

It stops short of the goal. Graph the speed again. A trace that never reaches the flat top means the limits are past what the drivetrain can do, so lower 2.5 and 3.0. A trace that does reach it means the gains are too small.

A command with an ending is a step, and Command.sequence(...) takes steps. Dynamic Path Planning uses this one as the final approach, after a planner handles the trip across open field.