My experimants with the PID controller have been going extremely well.
I have removed the "feed forward" terms from the program (adjusting P, I and D terms is more than enough for me to deal with) and I have taken the "display variables" routine out of the PID loop, replacing it with an "If...Then" statement based on the state is a push button connectred to RC1 (Pin 9).
My current circuit diagram is incPID_16F684_V5.pdf
My current program is as follows:
Code:
' Name: incPID_16F684_v3.pbp
' Date: 14th June, 2012
' Author: Barry Phillips
' AN0 (Pin 13) = Set Point
' AN1 (Pin 12) = Kp
' AN2 (Pin 11) = Kd
' AN4 (Pin 10) - Ki
' RA5 (Pin 2) = LCD Out
' RA4 (Pin 3) = Feedback Input (Tacho)
' CCP1 (Pin 5) = HPWM Ouptut
' RC1 (Pin 9) = Display Button
' Define DEBUG registers and bits
DEFINE DEBUG_REG PORTA
DEFINE DEBUG_BIT 5
DEFINE DEBUG_BAUD 9600
DEFINE DEBUG_MODE 1
CMCON0 = %00000111 ' Disable comparators
ANSEL = %01010111 ' Set AN0, AN1, AN2, AN4 and AN6 as analogue inputs
TRISA = %00010111 ' Set PORTA.0 - PORTA.4 to input
TRISC = %00000010 ' Set PORTC.1 as input
ADCON1 = %01010000 ' Set A/D clock Fosc/16
ADCON0 = %10000001 ' Right justify result
' Set Vref = VDD
' Select channel AN0
' Turn on A/D converter
OSCCON = %01110101 ' Set clock frequency to 8MHz
define OSC 8
' Define ADCIN parameters
Define ADC_BITS 10 ' Set number of bits in result
Define ADC_CLOCK 3 ' Set clock source (3=rc)
Define ADC_SAMPLEUS 50 ' Set sampling time in uS
Pause 1000
'***************** PID-Filter "external" variables *****************************
'***** These are the variables that the main application uses. *****************
'*******************************************************************************
pid_Status var byte 'Status of the PID-filter can be read here.
pid_Status_Out_Sat var pid_Status.7 'Flag for saturated output
pid_Status_I_Sat var pid_status.6 'Flag for saturated Integral drive.
pid_Kp_low var word
pid_Kp_high var word
SO con 5
pid_Kp Var Word 'Proportional gain.
pid_Ki Var Word 'Integral gain.
pid_Kd Var Word 'Derivative gain.
pid_Vel_Kff VAR WORD 'Velocity feedforward gain, set to 0 if not used!
pid_Acc_Kff VAR WORD 'Acceleration feeforward gain, set to 0 if not used!
pid_Error Var Word 'PID Filter error input. (setpoint - actual)
pid_Velocity_cmd VAR WORD 'Input of commanded velocity, set by the main application.
pid_Out Var Word 'PID Filter output (P + I + D + Vff + Aff)
pid_Ti var BYTE 'Time constant for the integral time
pid_I_Clamp var word 'Clamp for the integrator windup.
pid_Out_Clamp var word 'Clamp the final output to value in this var.
'*******************************************************************************
'******************* PID-Filter "internal" variables ***************************
'*******************************************************************************
pid_P Var Word 'Proportional drive.
pid_I VAR Word 'Integral drive.
pid_D Var Word 'Derivative drive.
pid_Vel_FF VAR WORD 'Velocity feedforward drive.
pid_Acc_FF VAR WORD 'Acceleration feedforward drive.
pid_LastVelocity_cmd VAR WORD 'Previous commanded velocity, automatically set by the PID routine.
pid_Sign Var BIT 'Keeps track of the sign of various values
pid_LastError var word 'Last error, used for calculating D drive
pid_Ei Var Word 'Integral drive accumulator.
pid_EiT VAR Word 'Temporary calculation variable
pid_IntCount Var Byte 'Counter for the integrator, matches against Ti
'*******************************************************************************
'************************** Initialise variables *******************************
'*******************************************************************************
pid_P = 0 'Reset P, I, D & feedforward variables.
pid_I = 0
pid_D = 0
pid_Velocity_cmd = 0
pid_LastVelocity_cmd = 0
pid_Kp = 128 'Set Kp (was $0700)
pid_Ki = 64 'Set Ki (was $0080)
pid_Kd = 118 'Set Kd (was $0225)
pid_Ti = 1 'Update I-term every 5th call to PID
pid_I_Clamp = 200 'Clamp I-term to max 200
pid_Out_Clamp = 255 'Clamp the final output to 255
pid_Vel_Kff = 0 'Feedforward terms not used...
pid_Acc_Kff = 0 '... set feedforward parameters to 0
ADValue VAR Word 'Output Tacho
Setpoint Var Word 'Output from Speed Set Pot
debug $FE, $80 + 5, "BAZTRONICS"
DEBUG $FE, $94 + 2, "PID Motor Speed"
debug $FE, $D4 + 3, "Controller v3"
'*******************************************************************************
'*********************** ROUTINE STARTS HERE ***********************************
'*******************************************************************************
Start:
ADCIN 0, Setpoint 'Read channel 0 to Setpoint
If PORTC.1 = 0 then
adcin 1, pid_Kp 'Read channel 1 to pid_Kp
adcin 2, pid_Kd 'Read channel 2 to pid_Kp
adcin 4, pid_Ki 'Read channel 4 to pid_Kp
DEBUG $FE, $C0, "Kp: ", dec pid_Kp, " "
DEBUG $FE, $94, "Kd: ", dec pid_Kd, " "
DEBUG $FE, $D4, "Ki: ", dec pid_Ki, " ", dec ADValue*6, " rpm "
Endif
Count PORTA.4, 100, ADValue
pid_Error = Setpoint - ADValue 'Calculate the error
'*******************************************************************************
'************************** GOTO PID ROUTINE ***********************************
'*******************************************************************************
Gosub PID 'Result returned in pid_Drive
'*******************************************************************************
'********************TEST FOR VALID DRIVE CONDITIONS ***************************
'*******************************************************************************
IF pid_Out.15 THEN 'If negative drive is called for...
pid_out=0 '...set output to 0
pid_Ei=0 'And keep I-term from accumulating
ELSE 'and if positive drive is called for...
pid_Out = ABS pid_Out '...set it to absolute value of pid_Out
ENDIF
'*******************************************************************************
'******************** SEND HPWM SIGNAL TO Channel 1 (Pin 5) ********************
'*******************************************************************************
HPWM 1, pid_Out, 10000 'Set PWM output
' Pause 10 'Wait....
Goto Start '...and do it again.
PID: 'This is the entrypoint from the main app.
'*******************************************************************************
'**************************** P I D F I L T E R *****************************
'*******************************************************************************
'Calculate the proportional drive
pid_P = (ABS pid_Error) */ pid_Kp 'Multiply by the P-gain
If pid_Error.15 then pid_P = -pid_P 'Re-apply sign if pid_Error is neg
'Calculate the Integral drive
pid_Ei = pid_Ei + pid_Error 'Add error to acumulator.
pid_IntCount = pid_IntCount + 1 'Increment the reset-time counter.
If pid_IntCount >= pid_Ti then 'Is it time to update the I-term?
pid_EiT = pid_Ei 'Copy accumulator
pid_Sign = pid_EiT.15 'Save Sign
pid_EiT = ABS pid_EiT 'Work with positive numbers
pid_EiT = pid_EiT */ pid_Ki 'Multiply by Ki gain
pid_EiT = pid_EiT / pid_Ti 'Divide by the reset time
If pid_Sign then pid_EiT = -pid_EiT 'Re-apply sign
pid_I = pid_I + pid_EiT 'Update I drive
pid_Sign = pid_I.15 'Save Sign
pid_I = ABS pid_I 'Work with positive numbers
if pid_I >= pid_I_Clamp then 'I-term is saturated....
pid_Status_I_Sat = 1 'set pid_I_Clamp flag....
pid_I = pid_I_Clamp 'and clamp I-term to what user have set.
Endif
If pid_Sign then pid_I = -pid_I 'Re-apply sign
pid_IntCount = 0 'Reset the counter.
If pid_EiT > 0 then
pid_Ei = 0 'Reset the accumulator.
ENDIF
Endif
'Calculate the derivative drive
pid_D = pid_Error - pid_LastError
pid_Sign = pid_D.15 'Save Sign
pid_D = (ABS pid_D) */ pid_Kd 'Multiply by Kd gain
If pid_Sign then pid_D = -pid_D 'Re-apply sign.
DerivDone:
pid_LastError = pid_Error 'Store error for next D calc.
'*******************************************************************************
'Calculate the total drive.
pid_Out = pid_P + pid_D + pid_i
pid_Sign = pid_Out.15 'Save Sign
pid_Out = ABS pid_Out 'Convert from two's comp. to abs.
If pid_Out > pid_Out_Clamp then 'If output is saturated...
pid_Status_Out_Sat = 1 'set status bit and...
pid_out = pid_Out_Clamp 'clamp output.
Endif
If pid_Sign then pid_out = -pid_out 'Re-apply sign.
RETURN 'And return to sender.
One thing I have noticed is that the PWM output jitters, even when the motor is operating with no mechanical load. The variation is not extreme but I would like to understand why it is happening. The jitter is more noticable at lower PWM duty cycles. I suspect it may have something to do with the COUNT statement used to get ADValue (ie the number of pulses from the shaft encoder in 100ms). Is there a better way of doing this? Perhaps Darrel's interrupt routine should be used - if so, what interrupt handler and how should it be implemented? The pulse train from the photo-interrupter is relatively clean but only swings from about 0.6V to 4V. I plan to try and clean up this signal by feeding it through a schmitt trigger.
My final thought for this project is to include a motor speed display; RPM updated every 100ms using ADValue as the data source. My concern is that the delay associated with sending this information to the display may cause some issues with the response time of the PID routine. Am I asking too much of the 16F684 to handle PID routine and the display routine concurrently?
All comments and suggestions are gratefully welcomed.
Cheers
Barry
VK2XBP
incPID Routine - Help Needed
Bookmarks