|
Hey! It works! Super fancy. Here's the math:
You start with:
pwm(t) = P*e(t) + I*INTEGRAL(e(TAU)), where the integral goes from 0 to t.
Differentiate both sides:
......d (pwm).......P*d e(t)
(2) -------- = ---------- + I*e(t), by the F.T.C.!!!
.........dt.................dt
Let's approximate d(e)/dt and d(pwm)/dt by finding the slope of two points for a small time interval, like 0.001, which is my throttle sampling rate, and substitute it into equation (2). Also, use the old value for e(t). I probably could use errorNew too. I haven't tried that though.
(pwmNew - pwmOld)/deltaT = P*(errorNew - errorOld)/deltaT + I*errorOld
Now, multiply both sides by deltaT:
pwmNew - pwmOld = P*(errorNew - errorOld) + I*deltaT*errorOld
Now, solve for pwmNew:
(3) pwmNew = pwmOld + P*(errorNew - errorOld) + I*deltaT*errorOld
/////////////////////////////////////////////////////////////////////////////
So, we need good values for P and I. What I did was change the range of pwmDutyFine to [0, 32000] or so. However, I end up shifting it down so that it's in the range [0, 512], and that gives me the resolution to play with P and I. Letting P = 1 in equation (3) above really is letting P = 1/64, since pwmDutyFine gets shifted down from 32000 to 500 or whatever. I let 'I' be some other number. haha! I still need to fine tune the values. Here's the code:
errorNew = _throttlePos - _current;
pwmDutyFine += (errorNew - ((7*errorOld) >> 3));
errorOld = errorNew;
// under absurd worst case, pwmDutyFine could grow by close to 1000
if (pwmDutyFine > 31740)
pwmDutyFine = 31740;
else if (pwmDutyFine < 0)
pwmDutyFine = 0;
// Round the result.
if ((pwmDutyFine % 64) >= 32) {
pwmDuty = (pwmDutyFine >> 6) + 1;
}
else {
pwmDuty = pwmDutyFine >> 6;
}
OCR1A = pwmDuty;
|