DC Motor Closed Loop PD Control Code - used L293D

I've spent many, many hours looking into writing some DC motor closed-loop control code. There a lot of great sources of information out there but I never found everything in a complete package. I put this all together for my own application and left it in a fairly generic state below. The code is fully documented with references. It's too long to post so I have it here:

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

Here are the features of the implementation:

// DC Motor Closed-Loop PD Control code
// by Dennis Schissler - June, 2009  (further credit below)
// http://subzonepen.blogspot.com/
// 
// 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 in 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 5ms or 200Hz (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

I'm working on a very cool and unique project that makes use of this code. You can find info on that here:

http://subzonepen.blogspot.com/

Cheers!

Dennis