Motor speed control using arduino and quadrature encoders

Question

I am trying to get precise control over the speed of rover 5 based robot. It has four PWM controlled motors and 4 Optical Quadrature Encoders. I am using 4-channel motor controller with rover 5 chassis. I am using arduino Nano for control. I am able to read encoder INT output and change PWM based on pulse width to control speed. But, as a result, I am getting heavy oscillations in the control output. That makes, the robot to move in steps, as PWM is changing constantly. I need an algorithm which can minimize this ringing and have a smooth moving robot. Here is my arduino code snippet.

void setup() {
    Serial.begin(9600);
    init_motors();
    init_encoders();        
    req_speed[0] = 20;
    req_speed[1] = 20;
    req_speed[2] = 20;
    req_speed[3] = 20;
}

void loop() {
  update_encoders();
  update_motors();
}

void update_motors() {
  int i, err;
  unsigned long req_width;
  if(micros() - mtime > 2999) {
    mtime = micros();

    for(i=0; i<4; i++) {
      digitalWrite(pins_dir[i], req_speed[i]>0);
      if(mtime - change_time[i] > 50000ul && req_speed[i] != 0) {
        cur_pwm[i] += 5;
      } 
      if(req_speed[i] > 0)
        cur_err[i] = req_speed[i]*10  - cur_speed[i];
      else
        cur_err[i] = (-req_speed[i]*10)  - cur_speed[i];
      if(cur_err[i] > 0 && cur_pwm[i] < 255) {
        cur_pwm[i]++;
      } else if(cur_err[i] < 0 && cur_pwm[i] > 0) {
        cur_pwm[i]--;
      }
      analogWrite(pins_pwm[i], cur_pwm[i]);
    }
  }
}

void update_encoders() {
  int i;
  unsigned long w;
  enc_new = PINC & B00001111;
  unsigned long etime = micros();
  for (i=0; i<4; i++) {
    if((enc_old & (1 << i)) < (enc_new & (1 << i)))
    {
      w = (unsigned long)(((etime - change_time[i])));
      pulse_width[i] = (w + pulse_width_h1[i] + pulse_width_h2[i])/3;
      pulse_width_h2[i] = pulse_width_h1[i];
      pulse_width_h1[i] = pulse_width[i];
      change_time[i]=etime;
      pulse_count[i]++;
      cur_speed[i] = (3200000ul / pulse_width[i]);
    }
  }
  enc_old=enc_new;
}

Here req_speed is between -100 to 100, where sign indicates direction. Please consider all undefined variables as globals. I experimentally measured that, when motor is running at full speed, the pulse width is around 3200us.

Encoders' INT outputs (XOR of A and B) are connected to A0 thru A3. Motor PWM is connected to D3, D5, D6, D9.

Is the arduino not fast enough to catch up with 4 encoders? Is PinChangeInts better than polling?

Please let me suggest any improvements to this code and advice me about what am I missing here.

Answer 1

It looks to me that your control loop is essentially:

if speed is too slow:
    increase PWM duty cycle one unit
if speed is too fast:
    descreate PWM duty cycle one unit

As you have observed, this doesn't work so well. Your control loop cyclically overshoots the target speed.

The canonical solution to this sort of problem is a PID controller. The concept is essentially the same, measure a thing and compare it to a target to calculate an error. Then adjust something (in your case, PWM duty cycle) based on the error.

However, a PID controller has three error terms:

• P: the current error
• I: the integral of the error
• D: the derivative of the error

For each of these terms, the controller has programmed some gain. It then multiplies each term by the respective gain to calculate how much the control input (PWM duty cycle) should be changed. Properly tuned, this allows you to get a feedback loop that at first ramps up the duty cycle, then as your vehicle approaches the target speed, backs off smoothly so you get a minimum of overshoot.

Answer 2

I really think you should look at interrupts rather than polling at the very least. I am also doing a similar encoder project (albeit not for a robot) and I have decided to spend a few $ on a separate chip for doing the counting of pulses which I can then read on the analogue pins through a DAC.

This has several benefits:

• frees up the MCU to do other stuff
• means that you do not have to worry about the length of your code in the polling loop resulting in missed pulses
• you can decide to only read the encoder count when you need to

The chip that has been recommended to me is here

By using i.e. a Arduino analogue pin -> multi/demultiplexer -> DAC -> dedicated encoder interface chip you can free up a LOT of the arduino pins/have a lot more encoders ;)

Answer 3

You might need to slow down your PWM response when you have measured that the motor isn't running at the correct speed. I'm guessing that you calculate a target PWM duty cycle given the error. It's called rate control and is what folk traditionally refer to as the differential term in a three term controller. Basically a simple way of achieving this is to decide where you need to head but limit the amount per second that you change the PWM duty cycle.

Anyway, that's the view of an analogue engineer and someone only only writes code in emergencies!