incPID Routine - Help Needed
Hi All,
I am endeavouring to put together a closed loop motor speed controller using Henrick's "incPID" routine. The end use for this project is to re-power my 9x20 lathe using a treadmill motor (180Vdc 2HP). For my trial project I am using a 24Vdc 500W electric scooter motor. My ultimate goal will be to maintain constant motor speed under varying mechanical load conditions.
I have read everything I could find on this forum related to PIC control and I believe what I am trying to achieve should be "do-able."
I have successfully put together a program based on the 16F684. I have managed to write code that will read a speed pot setting via ADCIN (10 bit) on AN0 (Pin 13) and successfully drive the 24V motor via HPWM output on CCP1/P1A (Pin 5). The PWM signal is fed to the motor via a low-side MOSFET with a suitable transistor drive circuit. Having achieved this, I then went to the next step and tried to implement the PID routine. My code is as follows:
I have also attached a copy of the circuit diagramCode:' PicBasic Pro program to test incPIDv1.5 routine by ' Henrick Olsson using PIC16F684 ' Name: incPID 16F684.pbp ' Date: 2nd June, 2012 ' Author: Barry Phillips ' Connect analog input 1 to channel-0 (RA0) Pin 13 ' Connect analog input 2 to channel-1 (RA1) Pin 12 CMCON0 = %00000111 ' Disable comparators ANSEL = %00000011 ' Set AN0 and AN1 as analogue input, all other digital inputs ' 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 '***************** 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 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 = $0300 'Set Kp to 3.0 (was $0700) pid_Ki = $0000 'Set Ki to 0.0 (was $0080) pid_Kd = $0000 'Set Kd to 0.0 (was $0225) 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 pid_Vel_Kff = 0 'Feedforward terms not used... pid_Acc_Kff = 0 '... set feedforward parameters to 0 ADValue VAR Word 'Output from F-V (LM2907) Setpoint Var Word 'Output from Speed Set Pot Direction VAR PORTA.5 'Direction bit set to POTA.5 (Pin 2) '******************************************************************************* '*********************** ROUTINE STARTS HERE *********************************** '******************************************************************************* Start: ADCIN 0, Setpoint 'Read channel 0 to Setpoint ADCIN 1, ADValue 'Read channel 1 to ADValue pid_Error = Setpoint - ADValue 'Calculate the error '******************************************************************************* '********************* GOTO HENRICK'S 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 velocity feedforward pid_Vel_ff = (ABS pid_Velocity_cmd) */ pid_Vel_Kff 'Multiply absolute value of commanded velocity by the gain if pid_Velocity_cmd.15 then pid_Vel_FF = -pid_Vel_FF 'If commanded velocity is negative so is the feedforward 'Calculate acceleration feedforward pid_Acc_FF = pid_Velocity_cmd - pid_LastVelocity_Cmd 'Current commanded speed - previous commanded speed is the acc. If pid_Acc_ff.15 then 'If the result is negative... pid_Acc_FF = (ABS pid_acc_FF) */ pid_Acc_Kff 'we take the ABSolute value and multiply by the gain pid_Acc_FF = -pid_Acc_FF 'Then we restore the sign ELSE 'Result is positive pid_Acc_ff = pid_Acc_FF */ pid_Acc_Kff 'Multiply result by the gain ENDIF pid_LastVelocity_Cmd = pid_Velocity_Cmd 'And remember current commanded velocity for next loop. '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_Acc_FF + pid_Vel_FF + pid_P + pid_I + pid_D 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.
[attach]incPID_16F684[/attach]
Obviously, I am not having the success I want - otherwise I would not be telling you all this
I have tried to simplify my setup by eliminating the feedforward terms and by setting the integral and differential terms to zero. In essence, I am looking purely at the proportional control in the first instance and, once that is under control, I propose to tweak the system using the other available terms.
As shown in the code, I have set pid_Out for single quadrant control by setting any potential negative drive to zero. I have made various attempts using values of Kp ranging from $0100 to $0900 but in all cases I only get varying rates of "bang-bang" output (ie output switching from zero to pid_Out_Clamp). I thought of looking at some of the variables on a 16x2 LCD but decided this might create its own set of timing problems and decided against it. I am not sure where to go or what to do next. Does anyone have any suggestions?
One thing that has always confused/concerned me with the HPWM statement in this program; pid_Out is a WORD variable (16 bits) but the PBP manual states that duty cycle in the HPWM command only ranges from 0-255 (8 bits). Am I missing something here?
For the record, I am using PBP 3.0.1.1, MCS 5.0.0.0 and the Melabs U2 programmer.
I would greatly appreciate any assistance to help resolve my problems with this code.
Cheers
Barry (VK2XBP)
Sydney, Australia
Bookmarks