Servo skeleton with fast & slow interrupts
Check out the Pololu.c code here. This is very simple code to control two motors to run at the same speed. A 1 ms interrupt service routine (ISR) is used to check the encoders of two motors to see if the motors have moved. Note that if your encoder pulses come too quickly, you will miss counts by this simple polling, so this code is only appropriate for low encoder pulse rates. A 13.1 ms ISR calculates a control law: if encoder 1 is ahead of encoder 2, slow motor 1 and speed motor 2, and vice-versa.