i actually have a bit of code that may help you.
PHP Code:
unsigned char PIkDControl(char Kp, char Ki, char Kd, int error) {
static int errorTotal = 0; //there is a better way to do this... meh
static int prevError = 0;
int P = 0;
int I = 0;
int D = 0;
P = (error*Kp)/100;
I = (errorTotal*Ki)/100;
D = ((error - prevError)*Kd)/100;
errorTotal += error;
prevError = error;
return (Limit_Mix(2000+127+P+I-D));
}
There are a couple of other tweaks, but i edited them out. you should come across them whilst testing (=