1 Attachment(s)
PID-filter routine (2nd try).
Hi,
(Ok, lets try this again. The previous version contained a non working example which is not very suitable in the Example Section so it has been updated. Here goes...)
This is a basic PID filter routine in the form of an include file.
Instructions:
1) Download the file incPID.txt attached to this post
2) Rename it to incPID.pbp
3) Place it in your project folder.
Example code:
This is a short example demonstrating how the PID routine can be used. The PIC's CCP module drives a motor via a suitable driver chip such as the LMD 18200. The motor shaft is connected to a potentiometer which is feeding position information back to the PIC's ADC forming a closed loop.
Code:
' |--PIC CCP --> LMD18200 --> Motor --> Potentiomter --> PIC ADC --|
' | |
' |---<-----<-----<------ P I D F i l t e r <------<------<-------|
'// Set up hardware registers, ADC etc here (not shown) //
ADValue VAR WORD '<---This is your variable.
SetPoint VAR WORD '<---This is your variable.
Direction var PortB.7 '<---Direction bit to motor driver.
Setpoint = 512 '<---Set desired position.
INCLUDE "incPID.pbp" 'Include the PID routine.
'These variables are declared by the incPID routine but
'the user needs to assign values to them.
pid_Kp = $0700 'Set Kp to 7.0
pid_Ki = $0080 'Set Ki to 0.5
pid_Kd = $0225 'Set Kd to 2.14
pid_Ti = 8 'Update I-term every 8th call to PID
pid_I_Clamp = 100 'Clamp I-term to max ±100
pid_Out_Clamp = 511 'Clamp the final output to ±511
Start:
Gosub GetAD 'Get position from AD - NOT SHOWN HERE.
pid_Error = Setpoint - ADValue 'Calculate the error
Gosub PID 'Result returned in pid_Drive
Direction = pid_Out.15 'Set direction pin accordning to sign
pid_Out = ABS pid_Out 'Convert from two's comp. to absolute
HPWM 1, pid_Out, 10000 'Set PWM output
Pause 10 'Wait....
Goto Start '...and do it again.
The code does not check for overflow in the calculations so the user needs to make sure that the value sent to the PID routine doesn't cause an overflow based on the current P, I and D gains.
More detalied information can be found in the attached file.
A special thank you goes to Darrel Taylor for spotting a couple of misstakes in the example but most of all for streamlining the code further, reducing its footprint about 20%.
/Henrik Olsson.
pid stuff/theory question for Mr Olsson
.5 sec is too long to get speed reading, you may be able to timer/counter 1 pulse duration to get speed ref faster .
PID itself is hard to tune. Isn't each correction just how-much and how fast ? and slowing-down and reducing each correction until there is no overshoot ? I made an alternator/regulator controller that self-tuned by jointly reducing correction amount and slowing corrections (longer pauses) until there were no more overshoots. The output was the PWM to MOSFET. You could also adjust up/down the gain/time with a pot input ref.
Are you interested in PID discussion ?
don
amgen
PID SPEED CONTROL QUESTION FOR Mr Henrikolsson
Dear sir,
What should i change in INCPID file that it controls the speed of motor not position.
And also i want to put Kp Ki Kd values through potantiometers.How it is possible.?:)
QUESTION FOR Mr HENRIKOLLSON
Dear sir thank u for reply.
My question is that i can get 10 bit resolution of PWM output to control motor speed.
I use following code.
CCP1CON = %00001100 ' Set CCP1 to PWM
T2CON = %00000111 ' Turn on Timer2, Prescale=4
PR2 = 249 ' Set PR2 to get 1KHz out
CCP1CON.4 = pid_out.0 ' Store duty to registers as
CCP1CON.5 = pid_out.1 ' a 10-bit word
CCPR1L = pid_out >> 2
It will give 10bit PWM output at 16MHz crystal.
Now spose pid_Error=+5 then spose after calculation
pid_Out=100
pid_out.15=0 'DIRECTION BIT
same result will be if pid_Error=-5 but
pid_out.15=1
With both errors final PWM resolution is same but how can I use direction bit to change PWM output to increase or decrease motor speed according to requirement.?
If there is any OFFSET which must be used ?
My probe with problem of use incPID.pbp
The QEI reading work good with count for position from 0-1000.
The PPWM give good voltage and signal too from 0 - 1024 at 20KHz.
The Henrik PID rutine work good but only when position and setpoint is max +/-50 !
and realy how make to motor clip +/- when position = setposition in some range about +/- 25% as break in position.
My output driver have next logic :
with PWM 512 motor STOP
0<--------512----->1024
left <---- stop-----> right
fast slow stop slow fast
Here is my debug program:
Code:
'****************************************************************
' Definicije PIC-a 18F4431 *
'****************************************************************
DEFINE OSC 20
include "incPID.pbp"
'****************************************************************
' LCD DEFINE LCD 4X20 chr *
'****************************************************************
DEFINE LCD_DREG PORTD
DEFINE LCD_DBIT 4
DEFINE LCD_RSREG PORTC
DEFINE LCD_RSBIT 0
DEFINE LCD_EREG PORTC
DEFINE LCD_EBIT 1
DEFINE LCD_BITS 4
DEFINE LCD_LINES 4
DEFINE LCD_COMMANDUS 2000
DEFINE LCD_DATAUS 50
'****************************************************************
' Definicije Portova *
'****************************************************************
ADCON0=0
ANSEL0=0
TRISB = %00000000
PortA = 0
PortB = 0
PortC = 0
PortD = 0
PortC.3 = 1
'****************************************************************
' Definicije PPWM *
'****************************************************************
DTCON = 0
PTCON0 = %00000000
PTCON1 = %10000000
PTPERL=$FF
PTPERH=$00
PWMCON0 = %00100000
PWMCON1 = 1
OVDCOND = %11111111
'****************************************************************
' Definicije QEI encodera *
'****************************************************************
'QEICON = %10011000
'DFLTCON = %00111000
'MAXCNTL = %11001111
'MAXCNTH = %00000111
QEICON=%10001000
DFLTCON = %00111000
MAXCNTL = %11101000
MAXCNTH = %00000011
PosLow VAR BYTE
PosHigh VAR BYTE
PosTemp VAR BYTE
Position VAR WORD
POSCNTL = 0
POSCNTH = 0
'****************************************************************
' Definicije TMR0 *
'****************************************************************
T0CON = %11001000
INTCON.5 = 1
INTCON.1 = 0
INTCON.7 = 1
INTCON.4 = 0
INTCON.3 = 0
INTCON.2 = 0
INTCON2.2 = 1
RCON.7 = 1
TMR0L =254
'****************************************************************
' Definicije PID filtera *
'****************************************************************
x var word
y var word
ADValue VAR word '<---This is your variable.
Setpoint VAR WORD '<---This is your variable.
Direction var bit '<---Direction bit to motor driver
pid_Kp = $0700 'Set Kp to 7.0
pid_Ki = $0080 'Set Ki to 0.5
pid_Kd = $0225 'Set Kd to 2.14
pid_Ti = 8 'Update I-term every 8th call to PID
pid_I_Clamp = 100 'Clamp I-term to max ±100
pid_Out_Clamp = 511 'Clamp the final output to ±511
x = 0
y = 0
setpoint = 100
pause 250
lcdout $fe,1
'****************************************************************
' Osnovna petlja *
'****************************************************************
on interrupt goto PIDcalc
loop:
if portc.3 = 1 then setpoint = setpoint + 1
if setpoint > 1000 then setpoint = 0
LCDOUT $fe,128,"POSITION = ",dec position
LCDOUT $fe,192,"SETPOINT = ",dec setpoint
LCDOUT $fe,148,"PID = ",dec pid_out
LCDOUT $fe,212,"PWM = ",dec y
pause 10
lcdout $fe,1
goto loop
disable
PIDcalc:
INTCON.1 = 0
if INTCON.2 = 1 then
PosHigh = POSCNTH
PosLow = POSCNTL
PosTemp = POSCNTL
If PosLow - PosTemp = 0 then Goto Done
PosHigh = POSCNTH
PosLow = POSCNTL
Done:
Position = POSHIGH * 256 + PosLow
advalue = position
pid_Error = Setpoint - ADValue
Gosub PID
Direction = pid_Out.15
pid_out = ABS pid_Out
x = (512 - pid_Out) + 512
select case direction
case 1
y = pid_Out
case 0
y = x
end select
PDC0L = y.LowByte
PDC0H = y.HighByte
endif
INTCON.2 = 0
resume
enable
end
If any help pls.
Regards Robert
Hi Henrik again after little time
I runing your PID rutine in 18F4431 and output is locked antiphase , all work like you explain me . I use two interrupt's one for position reding - QEI module and second to calculate PID , in main loop is only increment of setpoint. No Usart , no DEBUG or LCDOUT.
From time to time not a linear in time it have some little noise which I can also see with scope in PWM out and I can't image from where is ?
Simple motor have jig - agitation in range of couple step's to +/- and after couple next step's it go linear.
I use only CW rotation not CCW all the time and PID becouse it good magnetic and precise break in position.
Can it be from bad tuning of P,I,D parameter's ?
Output amplifier is copy of HP UHU with 4 x IRF540N.
Simple I am lost - it is not disaster but meke me little problems on my machin.
Best regards
\ ROBERT