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