Problem implement "Henrik" incPID

[phoenix_1]

Hello to all ..
I have PMDC with quadrature encoder and reading encoder from 0-2000 work OK.Command for my logic for motor with Mosfets work on next way:
I use PWM with Duty from 0-1024 (10bit) at 19.2Khz.
Motor stay at place when Duty is 512.
0<---LEFT----512----RIGHT--->1024
fast<-----Slow---stay----slow---->fast
That is command and speed via one PWM signal ;-)
All that things work good.
But I have problem to implement "Henrik" PID rutine for position 0-2000.
If I set setpoint to 100 it work after that not....
calculation for mirror of duty is (512 - pid_out) + 512
Here is test code:

'****************************************************************
'                 Definicije PIC-a  18F4431                     *
'****************************************************************
    DEFINE OSC 20
    include "incPID.pbp"
'****************************************************************
'                         RS 232 HSEROUT 19200 kbps             *
'****************************************************************
    DEFINE HSER_RCSTA 90h                      ' Enable serial port & continuous receive
    DEFINE HSER_TXSTA 24h                      ' Enable transmit, BRGH = 1
    DEFINE HSER_SPBRG 64                       ' 19200 Baud @ 20MHz, 0.16%
    DEFINE HSER_CLROERR 1                      ' Clear overflow automatically
'****************************************************************
'                          Definicije Portova                   *
'****************************************************************    
    ADCON0=0                                   'Turn of ADC
    ANSEL0=0                                   'Turn of ADC
    PortA = 0                                  'Make Port A inputs Digital
    PortB = 0                                  'Make Port B inputs Digital
    PortC = 0                                  'Make Port C inputs Digital
    TRISB = %00000000                          'Port B all  outputs ON    
'****************************************************************
'                          Definicije QEI encodera              *
'**************************************************************** 
    QEICON = %10011000
    DFLTCON = %00111000                
    MAXCNTL = %11001111              
    MAXCNTH = %00000111
    PosLow VAR BYTE                             'Storage for the low byte of encoder count
    PosHigh VAR BYTE                            'Storage for the high byte of encoder count
    PosTemp VAR BYTE                            'Temporary storage for low byte of encder count
    Position VAR WORD                           'Actual encoder position, 16bit word.
    POSCNTL = 0                                 ' clear lowbyte of counter for start
    POSCNTH = 0                                 ' clear highbyte of counter for start 
   '****************************************************************
'                          Definicije PID filtera               *
'****************************************************************
    ADValue VAR byte                           '<---This is your variable.
    Setpoint VAR WORD                          '<---This is your variable.
    Direction var byte                          '<---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 = 512                       'Clamp the final output to ±511                   
    Setpoint = 100
'****************************************************************
'                          Osnovna petlja PID-a                 *
'****************************************************************
Start:
   Gosub Getposition
   advalue = position 
   pid_Error = Setpoint - ADValue               'Calculate the error
   Gosub PID                                    'Result returned in pid_Drive
   Direction = pid_Out.15                       'Set direction pin accordning to sign
   pid_Out = ABS pid_Out                        'Convert from two's comp. to absolute
   if direction  = 0 then hpwm 1,pid_out,19200                     ' run motor left
   if direction  = 1 then  hpwm 1,(512 - pid_out) + 512,19200 ' run motor right
   pause 10                                     '.......little pause for debug.
   hserout [dec pid_out]
   Goto Start                                   '.......again.
'****************************************************************
'                          Petlja QEI-a                         *
'**************************************************************** 
GetPosition:
    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
Done: 
    Position = POSHIGH * 256 + PosLow           'Put high and lowbyte together.
    RETURN 
    End

If any suggest pls.
The Best regards Robert.