PID control

Calculate the error#

Error is the target minus the measurement. For a turn, both values are angles. Wrap the difference to −180°…180° before calculating the motor output. A proportional controller multiplies that error by kP.

Large kP produces more output for the same error. Too little may leave the robot short of the target; too much can cause overshoot or oscillation. The sign is just as important: positive error must produce a turn that reduces that error.

error = wrap180(target heading − measured heading)

References: Purdue SIGBots: PID Controller (opens in a new tab) · VEX Robotics: Inertial Sensor — C++ API (opens in a new tab)

P, I, and D#

This implementation differentiates the measurement instead of the target error. For a fixed target it provides damping without a derivative kick when a new target is requested. The heading delta is wrapped before dividing by the measured timestep.

Here dt is in seconds, angles are in degrees, and output is percent. kP therefore has units of percent/degree; kI is percent/(degree·second); kD is percent·second/degree. Gains from code that omits dt will not transfer directly.

References: Purdue SIGBots: PID Controller (opens in a new tab)

A turn PID function#

Add turnPid() above autonomous() in the shared main.cpp, then call it in place of turnToHeading(). The numeric gains are examples for testing, not recommended values for an unknown robot. Start with kI at zero.

Output is capped at 40%. Integral is restricted to a small error region, cleared on a sign change, and only accumulated when it does not worsen output saturation. Success also requires low angular speed for 150 ms; a brief pass through the target is not enough.

bool turnPid(double targetHeading, double timeoutMs = 2500) {
  if (!sensorsReady || !std::isfinite(targetHeading) || timeoutMs <= 0)
    { stopDrive(); return false; }
  const double kP = 0.65, kI = 0.0, kD = 0.035;
  double integral = 0;
  double previousError = wrap180(targetHeading - imu.heading(degrees));
  double previousHeading = imu.heading(degrees);
  double settledSeconds = 0;
  timer elapsed;
  elapsed.reset();
  double previousTime = elapsed.time(msec) / 1000.0;
  while (elapsed.time(msec) < timeoutMs) {
    wait(10, msec);
    double now = elapsed.time(msec) / 1000.0;
    double dt = now - previousTime;
    if (dt <= 0) continue;
    double heading = imu.heading(degrees);
    double error = wrap180(targetHeading - heading);
    double rate = wrap180(heading - previousHeading) / dt;
    if (std::fabs(error) > 15 || error * previousError < 0)
      integral = 0;
    double nextIntegral = std::fabs(error) <= 15
      ? limit(integral + error * dt, -40, 40) : 0.0;
    double unclampedOutput = kP * error + kI * nextIntegral - kD * rate;
    double output = limit(unclampedOutput, -40, 40);
    if (std::fabs(unclampedOutput) <= 40 || unclampedOutput * error < 0) integral = nextIntegral;
    leftDrive.spin(forward, output, pct);
    rightDrive.spin(forward, -output, pct);
    bool settled = std::fabs(error) < 1.5 && std::fabs(rate) < 8;
    settledSeconds = settled ? settledSeconds + dt : 0;
    if (settledSeconds >= 0.15) { stopDrive(); return true; }
    previousTime = now;
    previousHeading = heading;
    previousError = error;
  }
  stopDrive();
  return false;
}

References: VEX Robotics: Inertial Sensor — C++ API (opens in a new tab) · VEX Robotics: Motor and Motor Group — C++ API (opens in a new tab)

Tune the controller#

Record the target, final error, settling time, and whether the timeout was reached for each trial. Change one setting at a time. LemLib’s tuning guide is a useful comparison, but its gain values and exit-condition implementation are not the same as this function.

1. Keep kI and kD at zero. Use a low output limit and a repeatable 45° or 90° target.
2. Increase kP gradually until the robot reaches the target promptly. If it repeatedly oscillates, reduce kP.
3. Increase kD in small steps to reduce overshoot. Excessive D can amplify sensor noise or make motion sluggish.
4. Only add kI if the robot consistently stops short after the P/D behavior is reasonable. Keep the integral clamp and conditional integration.
5. Repeat short and long turns in both directions. Test with the robot’s normal mechanisms and battery conditions.

References: LemLib: PID Tuning (opens in a new tab)

Common problems#

Use PID for driving#

The same feedback structure can use inches of drive error instead of degrees. Start with the encoder conversion in Simple autonomous, then compute error from the target distance and measured travel. Retune the gains because the units and dynamics have changed.

Distance control and heading correction are separate jobs. A common extension computes a drive output from distance error and a turn correction from heading error, then mixes left = drive + turn and right = drive − turn. Normalize or cap the mixed commands so neither exceeds the allowed output.

References: PROS: Motors tutorial (opens in a new tab)