Electronic – arduino – Motor speed control using arduino and quadrature encoders

arduinocontrolencodermotorpwm

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.

Best Answer

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.

Related Topic