PID Control

[meatmanek]

Originally Posted by The Lucas

Also be careful reversing direction of motors in code (for simplicity I just swap wires)

Ahh, if only that were legal.
I can't quote a rule number, but I recall having trouble with inspection in 2004 due to that.

Anyway, there's another technique to have your I term go to 0 - integrate over time, rather than indefinitely. Keep a queue of error values and sum them every loop, or sum them as you go along and subtract values before you overwrite. This creates a term which is a hybrid of I and P. This will help overcome friction, but will not overcome gravity, springs, or other forces which are present when the system is at rest.

// Disclaimer: This code has not been tested. Read and think about it.
#define I_TIME 39
//I_TIME is the number of loops to integrate over
int iQPos = 0;
int iQueue[I_TIME]; // declare the queue

...
// Inside your initialization routine
for (iQPos=0; iQPos<I_TIME; iQPos++)
  iQueue[iQPos] = 0; // Initialize all to 0.

...
// inside of your PID code
I += error;
iQPos = (iQPos+1)%I_TIME; // Increment and loop if we get to the end.
I -= iQueue[iQPos];
iQueue[iQPos] = error;