Motion Profiling and Superstructure Coordination

Beyond a single PID

Advanced superstructures (an arm + elevator + wrist) must move fast without slamming, drawing brownout-level current, or colliding with themselves. The tools are motion profiles and coordinated state machines.

Motion profiles

A TrapezoidProfile generates a smooth position/velocity/acceleration trajectory between setpoints, ramp up to a max velocity, cruise, ramp down, bounded by max-velocity and max-acceleration constraints. ProfiledPIDController wraps a PID controller around this so the mechanism tracks a physically realistic path instead of a step:

var constraints = new TrapezoidProfile.Constraints(maxVel, maxAccel);
var controller = new ProfiledPIDController(kP, 0, kD, constraints);
// each loop:
double pidV = controller.calculate(measured, goal);
double ffV  = feedforward.calculate(
    controller.getSetpoint().position,   // for arm: angle
    controller.getSetpoint().velocity);
motor.setVoltage(pidV + ffV);

The feedforward acts on the profile's instantaneous setpoint, so the mechanism is always commanded the physically correct voltage for where the profile says it should be.

Tuning constraints

Start conservative, confirm clean tracking, then raise until the motion plot shows the actual curve diverging from the desired, that's your usable limit. Pushing past it just causes overshoot and current spikes.

Coordinating multiple DOF

With several joints, naive simultaneous moves can self-collide (e.g., the arm sweeps through the elevator's path). Solutions used by strong teams:

• State machine: define safe named states (STOWED, INTAKE, SCORE_HIGH) and legal transitions; the superstructure only moves through allowed intermediate states.
• Setpoint sequencing: hold one DOF until another clears a danger zone (e.g., raise the elevator before rotating the arm forward), expressed cleanly with command-based sequence/parallel/waitUntil.
• Soft limits / keep-out zones: clamp each joint's range as a function of the others so a bad command can't drive into a collision.

Worked pattern

A score sequence: (1) profile the elevator up to clear height, (2) waitUntil it clears, (3) in parallel profile the arm to the scoring angle and pre-spin a roller, (4) confirm all at-goal, (5) release the game piece. Each step uses profiled motion + feedforward, so the whole sequence is fast yet smooth.

The result

Profiled, coordinated superstructures cycle quickly and repeatably without brownouts or self-destruction, the difference between a robot that scores fast every match and one that occasionally crashes its own arm.