# Motor speed control using arduino and quadrature encoders

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.

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.

• Thanks. I looked up PID controller and looks like that is what I need here. I am considering the arduino PID library. I am still not sure how to tune the parameters kp, ki and kd. Any pointers on what values should I start with? – Punit Soni Nov 12 '13 at 2:41
• @PunitSoni there are many ways to go about tuning, but trial and error works pretty well. I'd rig something up with some knobs so you can easily adjust without reflashing the Arduino, at least until you find the right settings. Rule of thumb is that the kp term should do most of the work, make ki just big enough to eliminate any steady-state error, and use kd sparingly to reduce overshoot. You will quickly learn what settings work and which result in horrible instability. If you want to explore more rigorous approaches, just Google "PID tuning". There are entire books on the topic. – Phil Frost Nov 12 '13 at 3:59
• I tried the arduino PID library. From trial and error for the tuning, I was able to get some improvements from my original solution. But, its still the fluctuations in the speed is above acceptable levels around +/-20%. Parameters i used for this were (kp, ki, kd) = (1, 1.2, 0). I also think there are some issues with my speed measurement part. As, even with fixed pwm value, the encoder pulse_width varies a lot. If I count the number of pulses in 100ms, they seem to be fairly constant. But, that value is around 15 for full speed, seems to give poor resolution for speed – Punit Soni Nov 12 '13 at 21:50
• Your problem regarding poor resolution @ speed is because you are polling. See my answer and if you need more guidance on why you are seeing poor resolution at speed then let me know and I will expand – Paul Sullivan Mar 13 '14 at 19:08

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 ;)

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!

• In the arduino main loop. I am doing motor control every 3ms. Currently, the logic is very simple. If speed is less, increase pwm by one, if its more decrease it by one. I assumed, the range in which the pwm will fluctuate will be around +/-3, that would not cause much ringing, as its updated very fast. but, the result I see, is low frequency oscillations with around 100ms, that is very noticeable and performs very poorly. I think I need to use some standard control method for this use case rather than reinventing the wheel. I want to know, where I can get some existing work done on this. – Punit Soni Nov 11 '13 at 20:20
• @PunitSoni: Maybe you need to increase the PWM frequency to get finer control. – Ignacio Vazquez-Abrams Nov 11 '13 at 20:28
• Hmm, I did not think about the PWM frequency yet. I dont even know what is the default frequency arduino is using with PWM. Thanks, that is a good place to look at. – Punit Soni Nov 11 '13 at 20:33
• @PunitSoni Another thing to try is have the motor running at fixed speed and increment the PWM - see how much the motor speed changes - you might find that it changes about 10% and this is too much. Also, how long does it take to calculate your speed from the encoder? It may be that taking a bit longer will give you a much more accurate "take" on real speed. Maybe if you kept motor speed at constant pwm you could somehow measure what the encoder (and your algorithm) is producing. I'm talking about integrating your encoder signal and dividing by some value that gives a more precise average. – Andy aka Nov 11 '13 at 21:14