Gibby623,
A bit dubious about your PID function
Why do you have "PID" and "PID_comp"?? is "a" a tuning factor (as my "K") ??
I would have expected PID to be positive or negative according to the bot being tilted forward or backward
Also if your loop is fixed time, you can omit the dt factor
anycase, here is my PID code:
float K = 1.4;
int Kp = 3;
int Ki = 1;
int Kd = 6;
int last_error = 0;
int integrated_error = 0;
int pTerm = 0, iTerm = 0, dTerm = 0;
int updatePid(int targetPosition, int currentPosition) {
int error = targetPosition - currentPosition;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
return -constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}
This function accepts 2 parameters
- target position (normally = 0)
- current position = the angle obtained from Kalman or complementary filter
the function returns an integer value, between -255 and +255
Please let me know which Acc and Gyro models are buit on your specific Sparkfun board