incPID Routine - Help Needed

[Aussie Barry]

Hi Ioannis,

Thanks for your input, much appreciated.
As previously stated, this is the first stage of a much larger project - to re-power my lathe with a 180Vdc 2HP treadmill motor. Knowing that the final design will involve isolation between control and motor drive stages, I decided to replace my transistor gate drive circuit with an opto-isolated MOSFET driver (FOD3180). Also, I was experiencing lots of noise on my breadboard layout so keeping powr supplies isolated in my prototype development PCB can only be a benefit.

Attached is a copy of my new circuit diagram:

incPID_16F684_V4.pdf

JP1 & JP2 allow me to select one of two feedback sources; the analog output from the LM2907 (0-5V) or a pulse train from the shaft encoder (0-5.5kHz 50% duty cycle). My preference will be to use the pulse train and merely count pulses in my program to determine rotation speed but it was easy enough to fit both into my PCB design.

I have used multi-turn pots (VR2-VR6) to adjust the gain for the five different PID terms so, theoretically, I should be able to tune the system "on the fly". I will be able to display variables on the 4x20 LCD which will help me "see" what is happening.

The PCB (165mm x 115mm) has been exposed and etched and is ready for drilling and populating - hopefullt this will be completed tonight. I will keep you updated with more news soon.

Cheers
Barry

[Ioannis]

Just an observation. The shaft encoder is ready made or are you using an opto interrupter with a disc etc?

If the latter is the case be sure that it can detect pulses of 5.5KHz.

I think it is better to use directly the train pulse and feed a timer, TMR1 for example.

Better accuracy, less parts.

Ioannis

[Aussie Barry]

The shaft encoder is a photo interrupter with a disc - 100 steps per revolution.
I made the disc myself so I can adjust the number of steps quite easily.
I did a sanity check on the shaft encoder but counting the number of steps per second using a simple COUNT routine and displaying the result on the LCD. I then compared the result with a non-contact hand-held digital tacho. The two results were near identical - I was very happy with the accuracy of my system.

I have just finished assembling the PCB (see attached) and hope to do some testing on it tonight.




Cheers
Barry

[Ioannis]

Looks nice. An a big motor!

Wish you good luck Barry.

Ioannis

[Aussie Barry]

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:

' 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

[HenrikOlsson]

Hi Barry,
Any jitter on the input (ADValue variable in this case) or in the time between sampling the input will cause jitter on the output. If there's jitter on the input variable the motor probably IS changing speed slightly as I think the time that COUNT statement is actually counting pulses is pretty stable. However the time between samples may vary a bit in this case since you rely on the time of execution of the rest of the code.

Any jitter in the input signal gets "amplified" by the gain of the filter, especially the P and D gain. If you have P gain of 5 and a D gain of 10 then a difference of '1' on the input will cause the output jump '15' from one sample to the other. Now you're running very low gains so it shouldn't be too sensitive.

If the time between samples varies then a difference of x counts means different things. Lets say the count is 100, next sample it's 110 and the next it's 120. Now, lets say that the time between samples 1 and 2 is longer than between samples 2 and 3. This means that the motor is actually accelerating "harder" between samples 2 and 3 (the increase in number of pulses was the same but during a shorter period of time) but the filter doesn't know this.

The execution time of the PID routine may not be the same every time thru it and your DEBUG statements, when executed, will take quite a bit of time.

You're running the PID loop at less than 10Hz, I think that's a bit slow - I'd probably run atleast 10 times faster than that. (For my own motorcontrol project I'm running it at 1220-2800Hz).

What I'd do is to use DT-Ints to set up a timer interrupt at around 100Hz to begin with. I'd feed the pulses to another timer/counter and set that up to count the pulses in hardware instead of using the COUNT command. Each time the interrupt fires it grabs the count from the timers registers and subtracts the "last" count from it to get the "new" count. Then it feeds that to the PID routine.

Outputting data to the display or PC should be handled by the main routine. Unfortunatly the '684 doesn't have a UART so you're forced to use the bitbanged routine (SEROUT, DEBUG etc). If an interrupt fires during one of these statements the timing will get messed up and corrupt the data. Because of this I'd try to set up buffer and a system of semaphores (free_to_send, data_to_send, sending_data) and have it send one or two bytes between each interrupt to make sure it's got enough time to spit the bits out before the next interrupt.

/Henrik.

[Aussie Barry]

Hi Henrik,

Thank you once again for your very detailed response. Much appreciated.

I understand that my PID loop is running slow and appreciate that it would be best "to count the pulses in hardware instead of the COUNT command" BUT I need some direction on how to make this happen. I have had zero experience using timer interrupts (though I have worked with Darrel's DT-Interrupt for IOC). Can you walk me through the process or direct me to a previous thread which explains the process? I am really struggling to get my head around timer registers, where and how the pulses are stored and what needs to be done to retrieve them for implementation with the PID routine.

As far as the data outputting is concerned, I am quite happy to live with my "display on button press" routine just to see the filter gain terms after adjustment. I haven't played with a UART before so that will become a whole new learning curve for me. For the time being, I will be happy just to get the PID routine nice and stable. I can always program another PIC just to handle the RPM output routine...

Cheers
Barry
VK2XBP