Simple PID
T Chap
Posts: 4,223
After a few days of struggle with the PID obj, I finally started researching all I could online, looking at numerous PID code examples and reading up on the basic fundamentals. I ran across a code for some other processor that was the jist of what I needed to accomplish, and modified it for the Prop, made a few corrections to their code as well which had few major flaws. This simple code really made a lot of sense for me when trying to learn the process.
For anyone trying to learn about PID, this is a very simple program with some comments to help make sense out of the 3 part formula, you can also easily mute any of the 3 factors to see the effects of any one part. Surely this will need tweaking for use in any project, but I think it will make for a really good starer PID. This is in Spin only, does not use any floating point so only one cog used, also it is not set up as an object. Run the PID engine as is, it will start up as a learning program. I modified the PWMasm obj to use 0-1000 duty for smooth resolution.
Suggestions welcome.
Updated 10/08/08 Uploaded new uploaded version as well.
Post Edited (Originator) : 10/8/2008 8:38:55 PM GMT
For anyone trying to learn about PID, this is a very simple program with some comments to help make sense out of the 3 part formula, you can also easily mute any of the 3 factors to see the effects of any one part. Surely this will need tweaking for use in any project, but I think it will make for a really good starer PID. This is in Spin only, does not use any floating point so only one cog used, also it is not set up as an object. Run the PID engine as is, it will start up as a learning program. I modified the PWMasm obj to use 0-1000 duty for smooth resolution.
Suggestions welcome.
Updated 10/08/08 Uploaded new uploaded version as well.
'PID ENGINE Drives Direction pin, outputs absoloute errorto pwm.
CON
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
LCD_Pin = 14
LCD_Baud = 9600
LCD_Lines = 2
VAR
long kp, ki, kd, p, i, d, t,dt, tp, scale, duty, ilimit
long Darray
long Daverage
long encodercount
long output , error, Ierror, PrevError, maxoutput
OBJ
pwm : "pwmAsmDuty1000"
lcd : "debug_lcd"
encoder : "Rotary Encoder"
PUB start(pos)
LCDinit ' TURN OFF FOR MAIN PROGRAM
waitcnt(5_000_000 + cnt) 'seems to want a delay else strange number on lcd
'ZERO out any one factor P,I, or D to see the effects of p, i ,d . Or increase/decrease values to see the effects on the sum.
PIDinit(1000, 500, 100) 'a '0' mutes either p, i, or d from the total. P should carry greatest weight in many cases.
ilimit := 1000 'set integral limit
maxoutput := 1000
repeat
'cal(pos) 'use this if in object mode
caltest(pos) 'use this if in stand alone test mode
lcdloop 'test purpses only LCD
PUB caltest(pos) ' calclates PID and outputs 0 - 1000 duty this is a standalone test loop
error := (pos - encodercount) 'get error
if Ierror > ilimit 'set i limit max to avoid integral windup, using experimental values
Ierror := ilimit
if Ierror < -ilimit
Ierror := -ilimit
Darray := (error - PrevError)
Longmove(@Darray[noparse][[/noparse]0], @Darray, 4)
Daverage := (Darray[noparse][[/noparse]0] + Darray + Darray + Darray + Darray)/5
'If error => PrevError
'Ierror += error 'integral logic. add to itegral if error is stayin the same or increasing
'If error < PrevError 'accumulate integral value Ierror with error
'Ierror -= error 'reduce integral value Ierror if error is getting smaller
'this is an effort to reduce accumulation effect if the error is getting smaller
'3 constants kp, kd, ki allow the user to tune the error correction in the system, sent from caller
p := kp * error 'sets Proportional constant kp Note: proportional is simply multiplication x kp
i := ki * ierror 'sets integral value ( accumulative )
D := kd * Daverage 'sets derivative value using filter of 5 samples
'd := kd * (error - PrevError) 'sets derivative constatn kd, no filter, 1 sample
output := (p + i + d)/1000 'scale
if error > 0
outa[noparse][[/noparse]17] := 1 'if calculate returns positive number, direction if forward set dir pin high
if error < 0
outa[noparse][[/noparse]17] := 0 'if calculate returns positive number, direction if forward set dir pin low
if error > 1000
output := 1000
if error < -1000
output := - 1000
if error == 0 'reset ierror if error = 0(position reached)
ierror := 0
if output > 0
outa[noparse][[/noparse]17] := 1 'if calculate returns positive number, direction if forward set dir pin high
if output < 0
outa[noparse][[/noparse]17] := 0 'if calculate returns positive number, direction if forward set dir pin low
PrevError := error 'store error for next loop use
duty := ||output
pwm.SetDuty(duty) 'send output to pwm ( 0 - 1000) Note: in pwmasm obj, change SetDuty(counts)
PUB cal(pos) 'for use as object called by antoher program, turn off LCD if using another calling using LCD
error := (long[noparse][[/noparse]pos] - encodercount) 'get error
if Ierror > ilimit 'set i limit max to avoid integral windup, using experimental values
Ierror := ilimit
if Ierror < -ilimit
Ierror := -ilimit
Darray := (error - PrevError)
Longmove(@Darray[noparse][[/noparse]0], @Darray, 4)
Daverage := (Darray[noparse][[/noparse]0] + Darray + Darray + Darray + Darray)/5
If error => PrevError 'integral logic. add to itegral if error is stayin the same or increasing
Ierror += error 'accumulate integral value Ierror with error
If error < PrevError
Ierror -= error 'reduce integral value Ierror if error is getting smaller
'this is an effort to reduce accumulation effect if the error is getting smaller
'3 constants kp, kd, ki allow the user to tune the error correction in the system, sent from caller
p := kp * error 'sets Proportional constant kp Note: proportional is simply multiplication x kp
i := ki * ierror 'sets integral value ( accumulative )
D := kd * Daverage 'sets derivative value using filter of 5 samples
'd := kd * (error - PrevError) 'sets derivative constatn kd, no filter, 1 sample
output := (p + i + d)/1000 'scale
if error > 0
outa[noparse][[/noparse]17] := 1 'if calculate returns positive number, direction if forward set dir pin high
if error < 0
outa[noparse][[/noparse]17] := 0 'if calculate returns positive number, direction if forward set dir pin low
if output > 1000
output := 1000
if output < -1000
output := - 1000
if error == 0 'reset ierror if error = 0(position reached)
ierror := 0
if output > 0
outa[noparse][[/noparse]17] := 1 'if calculate returns positive number, direction if forward set dir pin high
if output < 0
outa[noparse][[/noparse]17] := 0 'if calculate returns positive number, direction if forward set dir pin low
PrevError := error 'store error for next loop use
duty := ||output
pwm.SetDuty(duty) 'send output to pwm ( 0 - 1000) Note: in pwmasm obj, change SetDuty(counts)
PUB PIDinit(kpp, kii, kdd)
dira[noparse][[/noparse]17] := 1
kp := kpp
ki := kii
kd := kdd
encoder.start(21, 1, 0, @encodercount)
pwm.Start(27) '
pwm.SetPeriod(10_000)
pwm.SetDuty(0)
pub lcdLOOP
lcd.gotoxy(0,0)
lcd.decf(encodercount, 4)
lcd.gotoxy(8,0)
lcd.decf(error, 8)
lcd.gotoxy(0, 1)
lcd.decf(((duty)), 6)
lcd.gotoxy(8,1)
'lcd.decf(d, 6)
lcd.decf(ierror, 6)
PUB LCDinit
lcd.start(LCD_PIN, LCD_BAUD, LCD_LINES) ' start lcd
lcd.cursor(0) ' cursor off
lcd.backLight(true) ' backlight on (if available)
lcd.cls ' clear the lcd
Post Edited (Originator) : 10/8/2008 8:38:55 PM GMT
Comments
- Got your I's and D's a bit mixed up in the main equations. Not that it affects anything.
- Ditch those absolute operators. They will cause inverted control.
Is that resetting of Ierror also in the original text you copied from? It might cause some strange step changes.
Bigger issues:
- You'll have to be careful with integral wind-up. Might need to scale things down a bit.
- Derivative response works much better if it is spread over a definable period. Not just one sample.
Scaling down the final calculated demand (output) hopefully will suffice for dealing with integral wind-up. Say, divide by 1000. And scaling up the KP and KD parameters accordingly.
I've, in the past, used a simple input filter to spread the derivative response.
The code they had would not ever stop accumulating even if error was zero, so I put in the resets. Otherwise unless ki was 0 (the multiplier), there was always accumulation, not sure why they had that error as I took it from a very detailed tutorial. The i and d equations came from the same tutorial though, and you state that I have them reverses, so maybe they never really tested the code before posting.
If you don't mind, I would really appreciate hearing what you mean. Are you saying take X samples of time and get an average? I don't quite have enough experience to have a reference for 'input filter' in this instance as you suggested. I suppose the analog would be a cap to smooth out spikes, so the digital would be an average.
The scale you see posted in that has nothing to do with real world application yet, the original code did show a divisor under p+i+d of 1 just to indicate scaling. Until I get the boards back, I am only testing with the encoder and led's for pwm and direction, checking the pwm on a scope as well.
Thanks for the suggestions!
Post Edited (Originator) : 10/7/2008 4:19:58 PM GMT
Well I can't speak for him, but I'm pretty sure he means to divide the change in value by the time elapsed since the last sample.
If your samples are *guaranteed* to be perfectly evenly spaced, you can skip this step. This is actually possible with assembly if you take advantage of the propeller's determinism. Since you use spin, it's better to be safe and just do the division.
Don't forget to update your weights!
I switched the i and d equations that were posted in reverse.
Thanks guys.
Post Edited (Originator) : 10/7/2008 8:35:35 PM GMT
I just noticed the direction detection is at the start of the code. I might have led you wrong on removing the absolute operator. I suggest, instead of just putting it back in in the various places, put the two parts (Direction select + absolute translation) together just before calling the PWM output routine.
Notable changes :
1. added Integral limit max to reduce windup issues
2. added integral logic, accumulates if error is => previous error. reduces integral value if error < PrevError
2. added scale factor under sum or output
3. removed || absolute values except for output to pwm obj.
As far as multiple sums, maybe something like this off the top of my head:
Post Edited (Originator) : 10/8/2008 7:24:23 PM GMT
DAT 'assembly cog which updates the PWM cycle on APIN 'for audio PWM, fundamental freq which must be out of auditory range (period < 50µS) org entry mov t1,par 'get first parameter rdlong value, t1 add t1,#4 rdlong pinOut, t1 or dira, pinOut ' set pinOut to output add t1, #4 rdlong ctraval, t1 mov ctra, ctraval 'establish counter A mode and APIN add t1, #4 rdlong period, t1 mov frqa, #1 'set counter to increment 1 each cycle mov time, cnt 'record current time add time, period 'establish next period :loop rdlong value, par 'get an up to date pulse width waitcnt time, period 'wait until next period neg phsa, value 'back up phsa so that it trips "value" cycles from now jmp #:loop 'loop for next cycle period res 1 time res 1 value res 1 t1 res 1 pinOut res 1 ctraval res 1I was under the impression that PWM was in duty cycles meaning 0 to 100%. This modification you made complicates my understanding of the whole process. However I do see a use for more control. 0 to 10_000 would be great
My problem is where the duty was changed to 1000. Because the period is 1000 for a reason. I figured it was related to the clock divided by the period or something along these lines. So simply changing the duty max vvalue to an arbitrary number does not seem right.