Autonomous Control System — VEX IQ 2nd Gen
I built a full PID control system for my VEX IQ robot — not because I had to, but because open-loop driving lost me matches I should have won. This documents what I built, why it works, and everything that went wrong first.
| Platform | VEX IQ 2nd Generation |
| Wheel diameter | 650 mm |
| Wheel count | 4 wheels (front + rear) |
| Gear ratio | 1 : 1 — motor shaft drives wheel directly |
| Wheel circumference | ≈ 2042 mm (π × 650) |
| Track width | 2100 mm — left to right |
| Axle spacing | 1730 mm — front to rear |
| Left motor | PORT 6, reversed |
| Right motor | PORT 7, forward |
| Heading sensor | Built-in Brain Inertial (no external gyro) |
| Drive style | Left Arcade — left stick only, fwd ×1.0, turn ×0.65 |
Output scales with distance to goal. 500 mm away → full power. 20 mm away → almost nothing. This is why the robot slows down automatically — no separate deceleration code needed.
Accumulates error over time. If friction keeps the robot 15 mm short, the integral grows until it pushes through. Can also cause oscillation if set too high — so I kept it off for most cases.
Reacts to how fast the error is changing. If the robot is closing in fast, D kicks in as a brake. Without it, the robot shoots past the target and oscillates. Higher kd = harder stop.
The drive function actually runs two PID loops simultaneously: one for distance (encoder → mm), and a second for heading (gyro → degrees). The heading correction is subtracted from one motor and added to the other — so the robot stays straight even if one side is slightly stronger.
I've watched robots miss a game element by 4 cm in autonomous and lose the match by that margin. PID keeps positional error under 20 mm on every run — that's the difference between picking up an object and driving past it.
A timed drive that works perfectly at 100% battery will stop 10–15 cm short by your fifth match of the day. PID doesn't care — it reads encoders, not time, so it drives the same distance regardless of voltage.
Over 1000 mm, a 2° heading error puts the robot almost 4 cm to the side. The gyro correction loop runs 50 times per second and trims left/right power in real time. The robot tracks straight even on rough tiles.
At the world level, you need both. My setup lets me drive manually with Left Arcade, hit a button to run a precise autonomous sequence, and the robot returns to manual control automatically when it's done.
Going to worlds three times taught me that the robots that win aren't always the fastest or the strongest. They're the ones that do the same thing correctly every single match.
struct PIDController { double kp, ki, kd, maxOutput, integralLimit; double integral = 0, prevError = 0; bool firstRun = true; double calculate(double error) { integral += error; // windup guard if (integral > integralLimit) integral = integralLimit; if (integral < -integralLimit) integral = -integralLimit; double deriv = firstRun ? 0.0 : (error - prevError); firstRun = false; prevError = error; double out = kp*error + ki*integral + kd*deriv; if (out > maxOutput) out = maxOutput; if (out < -maxOutput) out = -maxOutput; return out; } };
void driveStraightMM(double targetMM, double maxPower = 65.0) { Left.setPosition(0, degrees); Right.setPosition(0, degrees); double lockHeading = BrainInertial.heading(); PIDController drive {0.12, 0.0, 0.25, maxPower, 100.0}; PIDController heading{1.0, 0.0, 0.1, 25.0, 20.0}; double goal = targetMM * 1.05; // friction compensation int settled = 0; while (true) { double traveled = (Left.position(degrees) + Right.position(degrees)) / 2.0 / 360.0 * WHEEL_CIRCUM; double power = drive.calculate(goal - traveled); double steer = heading.calculate( normalizeAngle(lockHeading - BrainInertial.heading())); Left.spin(forward, power - steer, percent); Right.spin(forward, power + steer, percent); if (fabs(goal - traveled) < 20.0) settled++; else settled = 0; if (settled > 10) break; // stable for 200 ms wait(20, msec); } Left.stop(brake); Right.stop(brake); }
void turnDeg(double angleDeg, double maxPower = 60.0) { double target = BrainInertial.heading() + angleDeg; PIDController turn{0.4, 0.0, 0.8, maxPower, 30.0}; int settled = 0; while (true) { double err = normalizeAngle(target - BrainInertial.heading()); double out = turn.calculate(err); // minimum power — overcome static friction if (out > 0 && out < 12) out = 12; if (out < 0 && out > -12) out = -12; Left.spin(forward, -out, percent); // +angle = clockwise Right.spin(forward, out, percent); if (fabs(err) < 2.0) settled++; else settled = 0; if (settled > 10) break; wait(20, msec); } Left.stop(brake); Right.stop(brake); }
int main() { BrainInertial.calibrate(); while (BrainInertial.isCalibrating()) wait(50, msec); while (true) { // left arcade — normal driving double fwd = Controller.AxisA.position() * 1.0; double turn = Controller.AxisC.position() * 0.65; Left.spin(forward, fwd + turn, percent); Right.spin(forward, fwd - turn, percent); // hold ButtonLUp → run autonomous, then return to manual if (Controller.ButtonLUp.pressing()) { Left.stop(brake); Right.stop(brake); wait(200, msec); driveStraightMM(1000); wait(300, msec); turnDeg(90); wait(300, msec); driveStraightMM(500); } wait(20, msec); } }
Want the whole thing?
Every file above, fully commented — plus a full video walkthrough of the PID theory, a line-by-line code breakdown, 5 real bugs I hit while building it, and a slide deck to keep as a reference.
Questions about the build, the code, or VEX IQ in general? Reach out.
lucykim0907@gmail.com