Every VEX autonomous routine is a list of motions. Drive to this point, turn to this heading, drive to the next point. The simplest way to run that list is to finish each motion completely before starting the next one: the robot speeds up, cruises, slows to zero, settles, and only then starts the next motion from a dead stop. It's accurate. It also wastes a surprising amount of a 15-second autonomous period on slowing down and speeding back up.
Chaining, and the problem with it
LemLib, the motion library our team uses, already supports chaining motions together. You give a motion two extra parameters: earlyExitRange, how far from the target it's allowed to call itself done, and minSpeed, the speed it should hold on the way out. Set those well and the robot rolls from one motion into the next instead of stopping.
The catch is that both numbers depend on the motion. A long drive and a short hop need completely different values. So every motion in every routine had its own hand-tuned pair, and every time we changed a route, the tuning broke. I wanted the robot to work those numbers out itself.
How fast can it even get?
The first question is how fast the robot can possibly be going by the end of a motion. If it accelerates at a roughly constant rate a from a stop, the constant-acceleration equation from physics class answers that directly:
That's the ceiling for a motion of distance d. A short hop can't get anywhere near top speed, and a long drive can. Folding the acceleration and unit conversion into one constant, the lateral version in our code is sqrt(1000 * distance), capped at the motion's max speed. Then the exit speed is half of that:
Half is a compromise. It's enough that the robot carries real momentum into the next motion, but not so much that it can't still bend toward the next target.
When to let go
The early exit range is a fixed fraction of the distance, scaled by how fast the motion is allowed to go (127 is full power in VEX units):
A slow, careful motion barely exits early. A fast, long one starts handing off sooner. For drives, k has been somewhere around 15 to 20 percent. Turns use the same idea with angular distance and a smaller fraction, because overshooting a heading throws off every motion after it.
The code
All of this lives in a CustomChassis class that inherits from LemLib's Chassis, so it keeps everything LemLib already does and adds a couple of methods. Here's the drive version, trimmed slightly:
void chainToPoint(float x, float y, int timeout, ChainToPointParams params = {}) {
float dx = x - getPose().x, dy = y - getPose().y;
float distance = std::sqrt(dx * dx + dy * dy);
// fastest the robot can get over this distance, capped at max speed
float actualMax = std::min((float) std::abs(params.maxSpeed),
std::sqrt(1000 * distance));
float minSpeed = actualMax / 2;
float earlyExitRange = distance * 20 / 100 * actualMax / 127;
moveToPoint(x, y, timeout, {.forwards = params.forwards,
.maxSpeed = params.maxSpeed,
.minSpeed = minSpeed,
.earlyExitRange = earlyExitRange});
}A route is now just a list of chainToPoint and chainToHeading calls with coordinates and timeouts, and changing one waypoint doesn't mean re-tuning its neighbors.
See it
The simulation below runs the same four-point path both ways. It's simplified, with made-up acceleration limits instead of our robot's telemetry, but the shape of the difference is the point: one speed trace keeps dropping to zero, the other doesn't.
Fig. 1. Same path, two ways to drive it
This simulation needs JavaScript. The idea: instead of stopping at every waypoint, the robot leaves each motion early at half its peak speed and carries that speed into the next one.
At the Texas Region 3 Championship, our autonomous routines scored 80 autonomous points, the most of any team at the event. The full code is in the Push Back repo, and there's more about the team on the 285C page.