Closed-Loop Wheel Velocity Control With Encoders

PWM is a voltage, not a speed

analogWrite(motorPin, 180) does not ask for a speed. It asks for about 70% of the battery voltage, and what speed that produces depends on the load on the wheel, the friction in that gearbox, the state of charge, and the temperature of the brushes.

Which is why a robot with two identical motors and identical PWM drives in an arc. They were never identical — one gearbox is tighter, one brush set is newer, and at the same voltage they run 3–8% apart. Over three metres that is a visible curve.

On a differential-drive robot you can paper over it with a trim constant. On a mecanum base you cannot, because the inverse kinematics hands the four wheels wildly different targets — in the worked example there, one wheel gets 0.020 m/s and another 0.580 m/s from the same command — and those ratios are what decide the direction the robot goes. A wheel that is 5% slow does not make the robot slightly slow; it makes it go somewhere else.

Closing the loop on each wheel’s measured speed is what fixes that, and it is the same fix for every robot with more than two wheels or any need to arrive somewhere.

Measure the speed you actually have

Start from quadrature encoder counts. The conversion is fixed geometry:

counts per wheel revolution = CPR_motor × gear ratio × 4     (×4 for quadrature edges)
metres per count            = 2πr / counts per revolution

For a 12 CPR motor encoder behind a 30:1 gearbox on 97 mm wheels: 12 × 30 × 4 = 1440 counts/rev, and 2π × 0.0485 / 1440 = 212 µm per count.

Then the part everybody gets wrong:

// DON'T: counts since last loop, divided by loop time.
float speed = (count - lastCount) * METRES_PER_COUNT / dt;

At 100 Hz and 0.05 m/s that is one count per sample. Your speed signal is 0, 212, 0, 212 µm — a square wave with ±100% ripple, and a D term will amplify it into a motor that buzzes. Three ways out, and you generally want the first:

1. Sample slower than you control. Compute velocity over 20 ms (50 Hz) and run the loop at 50 Hz. More counts per window, less quantisation, and no filter lag.
2. Measure the time between edges rather than the edges in a time. Excellent at low speed, awkward at high speed, and it needs interrupt timestamps.
3. Low-pass the velocity. Cheapest, and it costs you phase margin — the thing you have least of.

// A 50 Hz velocity estimate, with the counter read atomically because an
// encoder ISR is writing it.
noInterrupts();
long count = encoderCount;
interrupts();

float dt = (now - lastTime) * 1e-6f;             // micros(), so no ms rounding
float v = (count - lastCount) * METRES_PER_COUNT / dt;
lastCount = count; lastTime = now;

volatile long encoderCount and the noInterrupts() bracket are not optional: a 32-bit read is several instructions on an AVR, and an ISR landing in the middle of it hands you a number that was never true.

Feedforward first, PID second

The instinct is to reach for PID. Do the easy 90% first.

A DC motor’s steady-state speed is close to linear in applied voltage, so if you know the speed you want, you can calculate most of the PWM:

pwm ≈ kS·sign(v) + kV·v

• kV — the slope. How much PWM buys one m/s.
• kS — the intercept. The PWM needed just to break static friction, before the wheel moves at all.

Measure both in ten minutes:

1. Put the robot on blocks so the wheels spin free.
2. Raise PWM slowly until the wheel just starts turning. That is kS, typically 25–45 out of 255.
3. Step PWM to 100, 150, 200, 250 and record the steady speed at each.
4. Fit a straight line. Its slope is kV, and it should extrapolate back to roughly your kS.

Now the loop only has to correct the error between that estimate and reality — which is small, so the gains can be small, so the whole thing is stable and quiet:

float feedforward = kS * sign(target) + kV * target;
float output = feedforward + kP * error + kI * integral;

Without feedforward, the integrator has to build the entire command from zero every time the target changes. That is slow, it overshoots, and it is the reason so many hand-tuned motor loops end up with a huge kI and a robot that surges.

The controller

P and I. Not D.

struct WheelLoop {
  float kS, kV, kP, kI;
  float integral = 0, target = 0;

  int update(float measured, float dt) {
    float error = target - measured;
    float ff = kS * (target > 0 ? 1 : target < 0 ? -1 : 0) + kV * target;
    float unclamped = ff + kP * error + kI * (integral + error * dt);
    int   output    = (int)constrain(unclamped, -255, 255);

    // Conditional integration: only accumulate if the output is NOT saturated,
    // or if the error would wind the integrator back toward the middle.
    // Otherwise a stalled wheel builds a command it cannot use and then keeps
    // driving for a second after you let go of the stick.
    if (fabsf(unclamped) < 255.0f || (error * unclamped) < 0) integral += error * dt;

    return output;
  }
};

Why no derivative: the D term differentiates the velocity signal, which is already a difference of encoder counts. You are differentiating twice, which means amplifying quantisation noise twice, and on a wheel there is almost nothing for it to damp anyway. A velocity loop with PI and good feedforward is the standard answer, and adding D is one of the more common ways to make a working loop worse. (A position loop is different — see cascaded control.)

Anti-windup is the part you cannot skip. Hold the robot against a wall with a velocity command applied, and a naive integrator ramps to full scale in a couple of seconds. Let go and the robot bolts. Conditional integration, one if, fixes it.

Tuning, in the right order

1. Feedforward alone. kP = kI = 0. Command 0.3 m/s and measure what you get. Within about 10% means kV is right. Fix it before touching anything else.
2. kP up until it buzzes, then back off a third. You are listening for a whine that appears with the gain and disappears without it.
3. kI until the steady-state error is gone. A wheel with correct feedforward needs very little. Start at kP/10 per second and creep up.
4. Check it under load, not on blocks. The whole point is the wheel that is being asked for more.
5. Test all four together, driving a diagonal. That is the command with the biggest spread of wheel targets, and it is where an under-tuned wheel shows first.

The PID simulator is a faster way to build the intuition for steps 2 and 3 than a robot is, and the tuning guide has the general procedure.

Sample rate

The loop rate has to be well above the thing it is controlling. A small geared DC motor’s velocity settles with a time constant of roughly 50–150 ms; ten samples inside that time constant is the usual rule, so 50–200 Hz.

Run it on a fixed schedule, not “as fast as loop() goes”, because dt appears directly in both the velocity estimate and the integrator:

const unsigned long PERIOD_US = 10000;   // 100 Hz
static unsigned long next = 0;

void loop() {
  unsigned long now = micros();
  if ((long)(now - next) < 0) return;    // signed compare survives rollover
  next += PERIOD_US;
  // ... read encoders, run four loops, write four PWMs
}

The signed comparison is the same millis() rollover trap in its micros() form, where it bites after 71 minutes rather than 49 days.

What it buys you, measured

Same robot, same command, same floor:

That last row is the underrated one. A closed loop knows when it has failed: a wheel whose output is pinned at 255 and still short of target is reporting something useful, and you can put that on a telemetry line, refuse the command, or slow the whole robot down to keep the ratios honest.

Troubleshooting

Next

The wheels now deliver the speeds they are asked for. Point the whole thing where the driver means with field-oriented drive, and feed it smooth targets instead of steps with motion profiles. Then watch what a velocity loop can and cannot save you from in the mecanum drive simulator — a perfectly held wheel speed still means nothing if the floor lets go.