Friday, June 19, 2009

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

5 comments:

  1. This comment has been removed by a blog administrator.

    ReplyDelete
  2. This comment has been removed by a blog administrator.

    ReplyDelete
  3. This comment has been removed by a blog administrator.

    ReplyDelete
  4. Hi Dennis, you mentioned on the possibility of using two channels of the encoder to obtain better resolution or accuracy. From my understanding you are using the hardware counter to count from external source (in this case one of the channel of the encoder). Would like to know, how can i do it if were to use both channels (full quadrature) while using the only one hardware counter. Thanks in advance.

    ReplyDelete
  5. Hi Dennis,

    Was just wondering whether it would be possible to implement this code without the external timers?

    Many thanks
    Alex

    ReplyDelete