DC Motor Closed-Loop PD Control Code

Here is code to do closed-loop DC motor control with the Arduino and the L293D motor driver IC:

http://docs.google.com/View?id=dhjz44fg_8f6sh7tcj

Features:

-- Makes use of the L293 motor driver; may easily be adapted to another

driver

-- Motor pwm'ing implemented by direct control of the ATmega Timer2

-- Closed-loop feedback via encoder wheel on the DC motor

-- Hardware counter implemented using Timer1 of the ATmega for

high-frequency capture of encoder counts - no interrupt

necessary!

-- PD control system implemented for motor control

My main source of info for implementing motor pwm'ing with the L293D was

this document:

http://www.arduino.cc/playground/Main/DirectionalMotorControlWithAL293D

The author is not listed so I can't provide specific credit. The code is

not particularly well-documented so I have attempted to remedy that in

my own code below.

Here is a link to the ATmega48/88/168 datasheet. I happen to be using the ATmega168. This resource is invaluable in gaining an understanding of the

hardware counters/timers. All page # references below are to this document

unless otherwise specified.

http://www.atmel.com/dyn/resources/prod_documents/doc2545.pdf

Here is a link to the L293D motor driver datasheet

http://www.robokitsworld.com/datasheets/l293d.pdf

User 'mem' from the Arduino forum was very helpful in giving me tips on implementing a hardware counter for the motor encoder. This link in

particular was exactly what was needed:

http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1231326297/all

Other links that were helpful:

http://letsmakerobots.com/node/2074

http://thecodebender.com/journal/2009/2/21/we-just-cant-leave-things-well-enough-alone.html

http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1235060559/8#8

http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1234764073

http://abigmagnet.blogspot.com/2008/10/dc-motor-control-part-one.html

http://mil.ufl.edu/~achamber/servoPWMfaq.html

For the PD control: the P is for position and the D is for derivative (or

velocity). Other systems also make use of I (integral, which gives you a

PID controller) but it is not particularly helpful in this application.

You may want to explore it for your own application. Computing the PD

gains is left to you. These are highly dependent upon the specific motor,

load, encoder resolution, and sampling frequency. You could just knob

twiddle until you find a workable combination. There are also analytical

methods out there for determining these gains.

Specific application notes:

The code here is tailored for a system that drives a part along a slider

rod (linear motion). In my case, this is done via a worm gear and a

linkage. I have roughly a 40:1 gear ratio. A homing move is implemented

to find a hard stop at the end of travel. The part is repeatedly moved

away from and then back to the home position. I added control input for

experimental use. Slew distance, acceleration, acceleration ramp - all

of these may be specifically tailored to your own application.

My motor is a smallish DC motor with a 32v supply. The quadrature encoder

provides digital output. I am using only 1 of the outputs in this case.

You could use the 2nd output for additional accuracy. Note that some

encoders output an analog signal. I used one of these initially and was

able to square up the signal using a Schmitt trigger. However, I had

noise coupling problems as soon as any pwm was input to the motor which

yielded spurious encoder counts. I'm not an EE so I abandoned this for

the much easier-to-use digital output encoder. My motor/encoder may be

found in some printer and/or scanner products where DC motors are used.

The Timer1 counter has a 16-bit register so if you expect encoder counts

higher than 16 bits, you will need to deal with this in the code. My

application runs well below this so I did not need to account for it in

this code.

In the motor_forward subroutine, I zero out any positive move errors

since in my application all forward moves position the driven part to a

hard stop. You can remove this positive move error check and modification

if your application is different.

I am checking the encoder position every 3ms or 333Hz (sample_freq). I

have placed some debug lines within this sample countdown period to ensure

that we have sufficient processor bandwidth. It's best to apply control

at the highest frequency possible. If you have other interrupts or other

processes happening during the motor move, you will need to slow down the

sample frequency. Note that the encoder counts are being refreshed at the

speed of the counter (really fast). I am only speaking of the frequency by

which I am computing position and velocity errors and applying control

gains

My L293D / Arduino / Motor connections are as follows:

Arduino digital pin 5 to motor encoder output pin

Arduino digital pin 11 to L293D pin 7 (motor pwm)

Arduino digital pin 12 to L293D pin 2 (motor direction)

Motor + pin to L293D pin 6 (pins 3 & 6 may be swapped to flip

the motor direction)

Motor - pin to L293D pin 3

Motor encoder +5v and ground pins suitably connected

L293D pin 1 connected to +5v (enable)

L293D pins 4,5,12,13 connnected to ground

L293D pin 8 connected to +32v (motor power)

L293D pin 9 connected to +5v (logic power)

Filter caps added per: http://letsmakerobots.com/node/2074