incPID Routine - Help Needed

[Aussie Barry]

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:

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

I have also attached a copy of the circuit diagram
[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

[Aussie Barry]

Two things I have realised since sending the initial post:

1/. In the circuit diagram, U3 should be 7805, not 7812
2/. I forgot to mention that my feedback is gained from a photo-interrupter/encoder wheel configuration mounted to the motor shaft. This feeds an LM2907 - Frequesncy to Voltage chip. The output from the LM2907 is adjusted to give 0-5V (stop - full throttle).

Cheers
Barry

[HenrikOlsson]

Hi,
You seem to be approcahing the project in all the correct ways, what you describe is most certainly doable and I honestly can't see why the output would swing between the extreme ends like you explain, especially with a gain of only $0100.

Can you replace the feedback voltage with another pot? Then, with a gain of $0100, you should be able to adjust the output with EITHER the setpoint pot or the feedback pot. Dissconnect the motor and just observe the output untill you get that working. I'd also try to get the variables output etiher to a LCD like you say or over a serial line to the PC.

One thing I do see though is that you have the pid_Out_Clamp set to 511 but as you say the HPWM command only accepts a dutycycle of 8 bits (0-255) so I'd set the pid_Out_Clamp variable to 255. You could always try copying pid_Out to a BYTE-sized variable and use that varaible in the HPWM statement but I don't think that is the real problem.

Hang in there!

/Henrik.

[Aussie Barry]

Hi Henrik,

I have had a high degree of success with this project today.
I disconnected the motor and replaced the feedback voltage with another pot as you suggested. I was able to get some very nice PWM signals showing on the scope. I think a lot of my problems last night were caused by a dodgy MOSFET. It failed under load this morning and the whole system fired up nicely once it was replaced.

To tune the system I added two more pots; one for Kp and one for Kd. I also connected the LCD which helped me greatly in seeing what was happening. The motor I am using doesn't have alot of inertia so Kp and Kd terms are relatively small for a tightly tuned system response. So far, so good.

I am a little confused with your velocity feedforward section. pid_velocity_cmd is initially set to zero and your program comment states that this variable is "set by the main program." From what I can find, this variable is only used in the calculation for pid_Vel_ff and pid_Acc_FF. If this is the case, it will always remain zero (as will pid_Vel_FF, pid_Acc_FF and pid_LastVelocity_Cmd). Am I missing something?

For my trials I ended up setting pid_Vel_FF manually just so the output could overcome what little insertia the system does have.

There is still a fair bit left to do on this project but before I do much more I need to transfer this circuit from my breadboard to a PCB. I am sure a lot of the instability I am seeing on my scope can be attributed to the breadboard (switching currents, ground loops etc.)

Your return comments on the feedforward situation would be greatly appreciated.

Cheers
Barry

[HenrikOlsson]

Hi Barry,
Excellent news, I'm glad you're moving forward!

If you're going to use the feedforward terms you need to set pid_Velocity_cmd in your program before you call the pid filter routine, in the same way that you must set pid_Error. In your particular case you'd probably just do something like

pid_Error = Setpoint - ADValue
pid_Velocity_Cmd = Setpoint
GOSUB PID

pid_Velocity_Cmd is the "input variable" for the feedforward terms. pid_Vel_FF, pid_Acc_FF and pid_LastVelocity_Cmd are "internal" variables and you should not need to worry about those.

In my own use of the filter the setpoint isn't velocity but position so I needed to have a separate input variable for the feedforward. My program then calculates the velocity based on the difference in commanded position between each concecutive loop.

I hope that makes sense, otherwise let me know and I'll try again.

/Henrik

[Ioannis]

Hi Barry.
I was looking your schematic and if I may suggest, replacing Q1, Q2, R16 and R17 with a Mosfet Driver chip, like microchip's, would give you much better drive to the mosfet Q3.

If you have a scope, look at node of the two emitters. If it shows that the two transistors are both conducting for a short time you may be waisting energy and heat the drivers. Also the edge that drives the Mosfet may not be very steep.

Ioannis