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.
Bookmarks