incPID Routine - Help Needed

[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

[HenrikOlsson]

Hi Barry,
Lets take it one step at the time and see what we get.

The '684 has three timers, TMR0, TMR1 and TMR2. Since you're using the ECCP module to generate PWM (thru the HPWM command) it occupies TMR2 so we can't use that for anything else. That leaves us TMR0 and TMR1.

Looking at the datasheet sections for TMR0 and TMR1 we can see that they both can be configured as either a timer or as counter so we just need to decide which to use as our counter. TMR0 is only 8 bits so if we're going to use that as our counter we need to make sure to not overflow it. If you have 100 counts per revolution and 2500rpm we're getting 4167 counts per second. If we were to run the PID loop at 10Hz we'd get 416 counts between updates which clearly overflows the 8bit counter, we'd need to either lower the input frequency or increase the update rate - you get the idea.

Looking at the datasheet section for TMR0 we can see that it's controlled thru the 6 lower bits in OPTION_REG. All we really need to do to set up as a counter is to set the T0CS bit (OPTION_REG.5 = 1). Now TMR0 will count the pulses comming in on the T0CKI pin (pin 11).

TRISA.2 = 1		' Make TOCKI input
OPTION_REG.0 = 0	' Prescaler 1:1
OPTION_REG.1 = 0
OPTION_REG.2 = 0
OPTION_REG.3 = 1	' Precaler assigned to WDT
OPTION_REG.4 = 0	' Increment in low-to-high transition
OPTION_REG.5 = 1	' Clock on T0CKI-pin
OPTION_REG.6 = 0
OPTION_REG.7 = 1

At this point all you need to do is to read the count out of the TMR0 register:

newCount VAR BYTE
newCount = 0
Main:
  newCount = TMR0 - newCount
  DEBUG "TMR0 count: ", #TMR0, 13
  DEBUG "Count since last time: ", #newCount, 13, 13
  Pause 1000
Goto Main

Try this and/or a variation of it to see if you can get TMR0 to count the pulses in your application.

One final note, the idea with the incPID routine was for it be INCLUDEd in a program, like yours. There's nothing wrong with copy/pasting it like you've done but it may distract the view from your actual application.

/Henrik.

[Aussie Barry]

Thanks Henrik.

I will need to wade through this tomorrow...oops, it is already tomorrow!

I will get back to you after I have had some time to digest it and put it into action

Cheers
Barry
VK2XBP

[Aussie Barry]

Hi Henrik,

I loaded the following code into my'684 and wired it up on my breadboard with a 2 x 16 LCD.

' Define LCD registers and bits

Define LCD_DREG PORTC
Define LCD_DBIT 0
Define LCD_RSREG PORTA
Define LCD_RSBIT 5
Define LCD_EREG PORTC
Define LCD_EBIT 4
Define LCD_BITS 4
Define LCD_LINES 2

CMCON0 = %00000111  ' Disable comparators
ANSEL = %00000000  ' Set PORTA as digital I/O
TRISC = 0               ' Set PORTC as output
TRISA.1 = 0             ' Make TOCKI input
OPTION_REG.0 = 0        ' Prescaler 1:1
OPTION_REG.1 = 0        ' 
OPTION_REG.2 = 0        '
OPTION_REG.3 = 1        ' Prescaler assigned to WDT
OPTION_REG.4 = 0        ' Increment on low-to-high transition
OPTION_REG.5 = 1        ' Clock on T0CKI-pin
OPTION_REG.6 = 0        '
OPTION_REG.7 = 1        '
OSCCON = %01110101      ' Set clock frequency to 8MHz
define OSC  8
newCount    var byte
newCount = 0
 
Main:
    newCount = TMR0 - newCount
    lcdout $FE, 1
    LCDOUT "TMR0 Count: ", #TMR0
    LCDOUT $FE, $C0
    LCDOUT "Count since: ", #newCount
    Pause 1000
Goto Main

I adjusted the speed of the motor such that the pulse train frequency from the photo interruptor was under 200Hz.
Each loop of the main program gives me a valid reading for #TMR0 and #newCount but there is no correlation between the numbers.

To be honest, I don't really see how there could be. TMR0 doesn't appear to be reset anywhere so it should just roll over from 255 to 0 each time the maximum count is reached.

When/how does TMR0 start and stop counting?

You did say in your last post that we would take this "one step at a time."
First step completed - we are now counting pulses. What next?

If I understand the process correctly, we need to set up one timer (say TMR0) as an accurate interval timer (say 10ms) and the other timer (TMR1) as a counter for the pulses. As the interval timer ends its cycle, the value of the counter is returned as ADValue for the PID routine. Am I on the right path here?

I look forward to your "next step" suggestions.

Cheers
Barry
VK2XBP

[HenrikOlsson]

Hi Barry,
TMR0 starts counting as soon as you set to BE a counter (OPTION_REG.5=1) and it will keep counting the incoming pulses. It will overflow att 255->0. The idea was that, because both newCount and TMR0 is a BYTE, that it will/should automatically give you the correct result but I think I may have made a little mistake there.

Lets try this instead:

newCount VAR BYTE
oldCount VAR BYTE
newCount = 0
oldCount = 0

Main:
  newCount =  TMR0 - oldCount
  oldCount = TMR0
  LCDOUT "TMR0 Count: ", #TMR0
  LCDOUT $FE, $C0
  LCDOUT "Count since: ", #newCount
  Pause 1000
Goto Main

When we have TMR0 counting pulses reliably we'll move to the next step which will be set TMR1 up to generate an interrupt periodically (using DT Ints). This interrupt will then "capture" the current count, calculate the "speed" using the above code and run the PID-loop. But lets make sure we have TMR0 and the code above working properly first. Basically it should now display the frequency of the incoming signal but the timebase will be a bit off due to LCDOUT statements taking a bit of time on top of the PAUSE 1000.

/Henrik.