PID-filter routine (2nd try).

[phoenix_1]

The QEI reading work good with count for position from 0-1000.
The PPWM give good voltage and signal too from 0 - 1024 at 20KHz.
The Henrik PID rutine work good but only when position and setpoint is max +/-50 !
and realy how make to motor clip +/- when position = setposition in some range about +/- 25% as break in position.
My output driver have next logic :
with PWM 512 motor STOP
0<--------512----->1024
left <---- stop-----> right
fast slow stop slow fast

Here is my debug program:

'****************************************************************
'                 Definicije PIC-a  18F4431                     *
'****************************************************************
    DEFINE OSC 20
    include "incPID.pbp"
'****************************************************************
'                        LCD DEFINE LCD 4X20 chr             *
'****************************************************************
    DEFINE LCD_DREG PORTD
    DEFINE LCD_DBIT 4
    DEFINE LCD_RSREG PORTC
    DEFINE LCD_RSBIT 0
    DEFINE LCD_EREG PORTC
    DEFINE LCD_EBIT 1
    DEFINE LCD_BITS 4
    DEFINE LCD_LINES 4
    DEFINE LCD_COMMANDUS 2000
    DEFINE LCD_DATAUS 50
                 
'****************************************************************
'                          Definicije Portova                   *
'****************************************************************    
    ADCON0=0                                
    ANSEL0=0
    TRISB = %00000000
    PortA = 0                               
    PortB = 0                                  
    PortC = 0
    PortD = 0                                
    PortC.3 = 1                                 
                         
'****************************************************************
'                          Definicije  PPWM                     *
'****************************************************************   
    DTCON = 0 
    PTCON0 = %00000000 
    PTCON1 = %10000000 
    PTPERL=$FF      
    PTPERH=$00
    PWMCON0 = %00100000 
    PWMCON1 = 1 
    OVDCOND = %11111111                     
'****************************************************************
'                          Definicije QEI encodera              *
'**************************************************************** 
    'QEICON = %10011000
    'DFLTCON = %00111000                
    'MAXCNTL = %11001111              
    'MAXCNTH = %00000111
    QEICON=%10001000
    DFLTCON = %00111000                
    MAXCNTL = %11101000               
    MAXCNTH = %00000011               
    PosLow VAR BYTE                           
    PosHigh VAR BYTE                           
    PosTemp VAR BYTE                          
    Position VAR WORD                          
    POSCNTL = 0                                
    POSCNTH = 0
'****************************************************************
'                          Definicije  TMR0                     *
'****************************************************************     
    T0CON = %11001000
    INTCON.5 = 1
    INTCON.1 = 0
    INTCON.7 = 1
    INTCON.4 = 0
    INTCON.3 = 0
    INTCON.2 = 0
    INTCON2.2 = 1
    RCON.7 = 1
    TMR0L =254 
'****************************************************************
'                          Definicije PID filtera               *
'****************************************************************
    x var word
    y var word
    ADValue VAR word                           '<---This is your variable.
    Setpoint VAR WORD                          '<---This is your variable.
    Direction var bit                          '<---Direction bit to motor driver 
    pid_Kp = $0700                             'Set Kp to 7.0
    pid_Ki = $0080                             'Set Ki to 0.5
    pid_Kd = $0225                             'Set Kd to 2.14
    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
    x = 0
    y = 0
    setpoint = 100                               
    pause 250                                  
    lcdout $fe,1                               
'****************************************************************
'                          Osnovna petlja                       *
'****************************************************************
on interrupt goto PIDcalc
     
     loop:
     if portc.3 = 1 then setpoint = setpoint + 1
     if setpoint > 1000 then setpoint = 0
     LCDOUT $fe,128,"POSITION = ",dec position  
     LCDOUT $fe,192,"SETPOINT = ",dec setpoint  
     LCDOUT $fe,148,"PID = ",dec pid_out        
     LCDOUT $fe,212,"PWM = ",dec y              
     pause 10                                   
     lcdout $fe,1                               
     goto loop                                  
         
     disable
PIDcalc:
     INTCON.1 = 0                                
     if INTCON.2 = 1 then                        
     PosHigh = POSCNTH                           
     PosLow = POSCNTL                            
     PosTemp = POSCNTL                           
     If PosLow - PosTemp = 0 then Goto Done      
     PosHigh = POSCNTH                           
     PosLow = POSCNTL
Done: 
     Position = POSHIGH * 256 + PosLow           
     advalue = position 
     pid_Error = Setpoint - ADValue               
     Gosub PID                                    
     Direction = pid_Out.15                       
     pid_out = ABS pid_Out
     x = (512 - pid_Out) + 512
     select case direction
     case 1
     y = pid_Out
     case 0
     y = x
     end select
     PDC0L = y.LowByte
     PDC0H = y.HighByte
     endif
     INTCON.2 = 0                                 
     resume                                       
     enable
     end

If any help pls.
Regards Robert

[HenrikOlsson]

The Henrik PID rutine work good but only when position and setpoint is max +/-50 !

OK, so what does it do if the setpoint is 51 then?

I see you're using On Interrupt and it seems you're using TMR0 as the source for the interrupt. It would be good if you could comment that section a bit so we don't have to read thru the datasheet in order to figure out what you're trying to do.

If I haven't missed anything you're running TMR0 in 8bit mode, clocked from the internal clock @20/4=5Mhz, prescaler selection is set to 1:2 which means the timer is clocked at 2.5Mhz, overflowing and generating interrupts at 9765.625Hz. There's no need to run the PID-loop at that speed, generally 1000Hz is good enough for motor control.

The Pause 10 statement needs to go away, and the LCDOut statements will likely mess with the timing as well. Read up on how On Interrupt works in the PBP manual. I highly recomend you look up Darrels Instant Interrupt routines as they don't suffer from the things On Interrupt does.

What do you mean by

and realy how make to motor clip +/- when position = setposition in some range about +/- 25% as break in position

It's really hard to understnad what you mean..... A deadband where anything within 25% of the setpoint is good enough or a following error trip and when the error is bigger than a set amount?

I've just received my copy of v2.60 so I can start using 32bit variables. I'm having a bt of a problem getting it going though. On top of that I fried my servo development board by connecting 24V to the 5V input so I need to get that sorted....

[phoenix_1]

here is what I rewrite in incPID

'*******************************************************************************
'*******************************************************************************
'Calculate the total drive.

pid_Out = pid_P + pid_I + pid_D         'Calculate total drive....

 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        'Output is saturated...
if  pid_Out >= 640 then
    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.
SkipPID:

now the PID work close to good , ABS pid_error is in range close to 511 +/- 100 when I probe to personal move shaft of motor.
Becous my motor have gear 6.25:1 my variable ( y as LONG) go from 0-6250 for one full turn of shaft.Motor must go 6.25 * 1000.
QEI is setup for one turn of motor shaft go 0-1000.
It is not full good but it is close to be PID :-(
PBP code :

'****************************************************************
'                 Definicije PIC-a  18F4431                     *
'****************************************************************
    DEFINE OSC 20
    include "incPID.pbp"
'****************************************************************
'                         RS 232 HSEROUT 19200 kbps             *
'****************************************************************
    DEFINE LCD_DREG PORTD
    DEFINE LCD_DBIT 4
    DEFINE LCD_RSREG PORTC
    DEFINE LCD_RSBIT 0
    DEFINE LCD_EREG PORTC
    DEFINE LCD_EBIT 1
    DEFINE LCD_BITS 4
    DEFINE LCD_LINES 4
    DEFINE LCD_COMMANDUS 2000
    DEFINE LCD_DATAUS 50
                 
'****************************************************************
'                          Definicije Portova                   *
'****************************************************************    
    ADCON0=0                                
    ANSEL0=0
    TRISB = %00000000
    PortA = 0                               
    PortB = 0                                  
    PortC = 0
    PortD = 0                                
    PortC.3 = 1                                 
                         
'****************************************************************
'                          Definicije  PPWM                     *
'****************************************************************   
    DTCON = 0 
    PTCON0 = %00000000 
    PTCON1 = %10000000 
    PTPERL=$FF      
    PTPERH=$00
    PWMCON0 = %00100000 
    PWMCON1 = 1 
    OVDCOND = %11111111                     
'****************************************************************
'                          Definicije QEI encodera              *
'**************************************************************** 
    QEICON=%10001000
    DFLTCON = %00111000                ' enable error filter for all capture inputs
    MAXCNTL = %11101000               'low set for maxcnt of 12500 becouse have gear 6.25:1
    MAXCNTH = %00000011                'hig set for maxcnt of 12500 becouse have gear 6.25:1             
    PosLow VAR BYTE                           
    PosHigh VAR BYTE                           
    PosTemp VAR BYTE                          
    Position VAR WORD  
    POSCNTL = 0                        ' clear lowbyte of counter for start
    POSCNTH = 0                        ' clear highbyte of counter for start                         
'****************************************************************
'                          Definicije  TMR0                     *
'****************************************************************     
    T0CON = %11001111
    INTCON.5 = 1
    INTCON.1 = 0
    INTCON.7 = 1
    INTCON.4 = 0
    INTCON.3 = 0
    INTCON.2 = 0
    INTCON2.2 = 1
    RCON.7 = 1
    TMR0L =237       ' 8bit ,1:256 divider ,237 preset = 1000HZ at 20MHz clk
'****************************************************************
'                          Definicije PID filtera               *
'****************************************************************
    s var word
    f var word
    y var long                         'Total pulse count storage variable 32bit
    x var byte                         'Number of full shaft rotation of 360'
    z var BIT                          'Actual bit if any move of shaft to any direction
    h var BIT                          'direction bit 0 for left , 1 for right turn of encoder
    ADValue VAR word                           '<---This is your variable.
    Setpoint VAR WORD                          '<---This is your variable.
    Direction var bit                          '<---Direction bit to motor driver 
    pid_Kp = $0700                             'Set Kp to 7.0
    pid_Ki = $0080                             'Set Ki to 0.5
    pid_Kd = $0225                             'Set Kd to 2.14
    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
    s = 0
    f = 0    
    z = 0                                 
    x = 0 
    y = 0
    h = 0
    PIR3.2 = 0 
    setpoint = 0        
    pause 250                                  
    lcdout $fe,1                               
'****************************************************************
'                          Osnovna petlja                       *
'****************************************************************
on interrupt goto PIDcalc
    
     loop:
     if portc.3 = 1 then setpoint = setpoint + 1
     if setpoint > 6250 then setpoint = 0
     LCDOUT $fe,128,"POSITION = ",dec y  
     LCDOUT $fe,192,"SETPOINT = ",dec setpoint  
     LCDOUT $fe,148,"PID = ",dec pid_out        
     LCDOUT $fe,212,"PWM = ",dec f," PE = ",dec ABS pid_error
     LCDOUT $fe,128,"POSITION =     "     'clear line without use $fe,1
     LCDOUT $fe,192,"SETPOINT =     "     'clear line without use $fe,1

     goto loop                                  
         
     disable
PIDcalc:
     INTCON.1 = 0                                
     if INTCON.2 = 1 then                        
     z = 0
     PosHigh = POSCNTH                           'Get high byte of counter
     PosLow = POSCNTL                            'Get low byte of counter
     PosTemp = POSCNTL                           'Get low byte again  
     If PosLow - PosTemp = 0 then Goto Done      'Compare, if equal we're done.
     PosHigh = POSCNTH                           'If not, get values again.
     PosLow = POSCNTL
     z = 1 
Done: 
     Position = POSHIGH * 256 + PosLow           'Put high and lowbyte together. 
     h = QEICON.5                                'QEICON.5 for right = 1 for left = 0 
     if PIR3.2 = 1 and h =1 then x = x+ 1
     if x <> 0 and PIR3.2 = 1 and h =0 then x = x- 1
     PIR3.2 = 0                                  'PIR3.2 maxcount transition down or up was detected
     h = 0 
     y =  (1000 * x) + position                    ' increment  total counter
     if z = 1 and h = 0 then y = y -(1000-position) ' dectement  total counter 
     if y > 6250 then
     x = 0
     endif
     advalue = y
     pid_Error = Setpoint - ADValue
     gosub PID                                    
     Direction = pid_Out.15                       
     pid_out = ABS pid_Out
     s = (512 - pid_Out) + 512
     select case direction
     case 0
     f = pid_Out
     case 1
     f = s
     end select
     PDC0L = f.LowByte
     PDC0H = f.HighByte
     endif 
     INTCON.2 = 0                                 
     resume                                       
     enable
     end

[HenrikOlsson]

Hi Robert,
I'm sorry but I'm having a hard time understanding you. I like to help you but it's just hard when you don't even answer my questions. Now you say it's close to being PID? What exactly is it that doesn't work?

If the motor isn't "stiff" enough you need to tune the PID parameters, I see you have Kp, Ki & Kd exactly as in my example code - which is just that - an example. You need to change them to suit your application.

Also, you could as well changed your code to read pid_Out_Clamp = 640 instead of hardcoding 640 into the incPID routine. In any case if you only have 10bits of PWM resolution why allow the output to swing to +/-640 which is 11bits?

Another thing I notice is that you preset TMR 0 with the value 237, but you do that only at the beginning of the program, so it will go from 237->256, cause an interrupt, then it will start from 0 again. With the pescaler at 1:256 that will be an interrupt rate of 38Hz which might be a bit slow. You need to preset TMR0 again in your ISR.

You can change this:

advalue = y
pid_Error = Setpoint - ADValue

To simply read

pid_Error = Setpoint - y

It won't work any better but a little faster and it'll save you a byte or two ;-)

And, try removing the LCDOut statements as a test and see what impact that has on it. Do you have an oscilloscope? Connect it to a pin, set the pin high the first thing you do in the ISR and low again before returning - then you can see if you have the correct interrupt frequency.

[phoenix_1]

ok in UHU or any other PID control if POSITION= SETPOINT motor must clip +/- for to make break in position.
when you set SETPOINT increment +1 or +XXX then PID must move motor in that direction and when MOTOR come to SETPOINT PID must again clip motor +/- in new POSITION becous POSITION is again = SETPOINT.
If you probe to mechanic move MOTOR SHAFT from locked position PID rutine must give reaction to back motor to lock POSITION.
Your PID control work good maybe with L2XX motor driver chip but with real output driver like HP UHU output stage NOT work.
In uhu sistem you have two pins on output stage.One for motor enable / disable (logic 1 enable , logic 0 disable.That is use for motor protect with current detector via resistor in DRAIN of output MOSFETS.
Second pin is in same time PWM drive / direction.
When PWM on these pin is 50/50% that is 512 on 10bit then motor is in stop condition.When on these pin come PWM from 512-0 you have movement of motor from slow 511 to full spid PWM close 10.And when you go from 513 to 1024 you have slow to full speed of motor at close 1014.
My motor have gear 6.25 motor full turn for one shaft full turn.That mean I must use wariable for position storage from 0 - 6250 which represent full range for one full turn of output gear shaft.
When I use your PID rutine it break motor to go from lock position to minus CCW and it give PWM to motor 511 , 510,...492...400 and move motor CW to lock dead point.But when i move output shaft to CW position your PID giwe PWM 511 and normaly that must be to give 513,515....600 to mowe motor in CCW to motor come to lock point.That is becouse in your position is :

'*******************************************************************************
'*******************************************************************************
'Calculate the total drive.

pid_Out = pid_P + pid_I + pid_D         'Calculate total drive....

 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        'Output is saturated...
    pid_Status_Out_Sat = 1              'set status bit and...
     pid_out = pid_Out_Clamp '  pid_Out_Clamp = 511 

Endif
If pid_Sign then pid_out = -pid_out     'Re-apply sign.

RETURN                                  'And return to sender.
SkipPID:

Ok and your suggestions are good but I was spend 10..hours and my head is like baloon...sorry.
Regards Robert

[HenrikOlsson]

Hi Robert,
Don't worry, I know the difference between sign & magnitude and locked antiphase.

If you're working with locked anti-phase, like the UHU chip does, then you need to look at the output from the PID filter, if it's positive you add to the dutycycle and if it negative you subtract the ABS value of the PID_Out from the duty cycle. Basically....

'At startup set the PWM dutycycle to 50%
PID_Error = Position - Setpoint
Gosub PID

If PID_Out.15 = 1  then           'Output is negative 
  DutyCycle = 512 - ABS PID_Out
Else                           'Output is positive
  DutyCycle = 512 + PID_Out
Endif

So again, if your dutycycle register is 50% (512) when no torque is delivered (ie. you are working with locked antiphase) then you really should clamp the output of the PID to +/-511, if you don't do that the dutycycle register may over- or underflow. If, for example, the output of the PID filter is allowed to swing to -640, and you subtract that from 512 you end up with -128. -128 in a 16bit variable is the same as 65407 so if you take the lower 10 bits (=895) of that and stuff it into your dutycycle register you'll end up with a dutycycle of 87% instead of 0 - torque will get delivered in wrong direction.....

Also, with a powerstage such as the one on the HP-UHU you need to make sure that dutycycle never reaches 0 or 100% because then the bootstrap capacitors in the highside MOSFET drivers no longer have any time to recharge and bad things will happen.

And, to show you that it does work, here's a link to a video I just uploaded:


Yes, it uses sign & magnitude and no it's not step and direction. It takes serial commands from a host CPU or PC. I've also got a couple of videos there featuring the HP-UHU. I've been quite involved in the HP-UHU project so I know it pretty well.

And as a friendly note, if you think that this PID-code doesn't work for your application then by all means feel free to use something else.

Sincerely,
/Henrik.

[phoenix_1]

No I mean it work good and many thanks to you for writing that...
My best regards and I will continue to test it for my machine.
Regards Robert