Maybe someone will be optimise code maybe not.
Here is my last test and all my problems wos on my wrong seting of 18F4431
and wrong calc for position counter.
These code only mowe motor CW but I was tested also CCW and is ok.
My best regards to all
/Robert

Code:
@ __CONFIG    _CONFIG1H, _OSC_HSPLL_1H
@ __CONFIG    _CONFIG2H, _WDTEN_ON_2H & _WDPS_512_2H
@ __CONFIG    _CONFIG2L, _BOREN_OFF_2L & _PWRTEN_ON_2L    
@ __CONFIG    _CONFIG3H, _MCLRE_ON_3H
@ __CONFIG    _CONFIG4L, _LVP_OFF_4L 
     clear
     define OSC 40                 ;define osc speed 10MHz x 4 PLL = 40 MHz
     ADCON0= %00000000                                
     ANSEL0= %00000000
     ANSEL1= %00000000
     TRISA = %00011110
     TRISB = %00000000
     TRISC = %00000000
     TRISD = %00000000
     TRISE = %00000000
     LATA = %00000000 
     LATB = %00000000 
     LATC = %00000000 
     LATD = %00000000 
     LATE = %00000000 
     duty var word                  'duty variable for PWM CCP1 
     PosLow VAR BYTE                ; Temp variable for Position calc.           
     PosHigh VAR BYTE               ; Temp variable for Position calc.            
     PosTemp VAR BYTE               ; Temp variable for Position calc.
     position var word              ; Position counter final variable from QEI module
     direction var bit              ; Direction bit for motor CW or CCW
     setpoint var word              ; Setpoint of position variable
     preset_TMR1 var word           ; Timer1 prescaler variable
     include "PID.pbp"              ; Include Henrik PID rutine
     INCLUDE "DT_INTS-18.bas"       ; Base Interrupt System
     INCLUDE "ReEnterPBP-18.bas"    ; Include if using PBP interrupts
;----[High Priority Interrupts]-----------------------------------------------
ASM
INT_LIST  macro    ; IntSource,        Label,  Type, ResetFlag? 
        INT_Handler   TMR1_INT,  _pidloop,   PBP,  yes
    endm
    INT_CREATE                                    
ENDASM
     CCP1CON = %00001111
     T2CON = %00000100
     T1CON = %10000101 
     QEICON = %10001000 
     DFLTCON = %00111010
     POSCNTH=0                                    ; set initial counter for encoder, H bit
     POSCNTL=0                                    ; set initial counter for encoder, L bit
     setpoint = 0                                 ; set initial for Setpoint
     duty = 512                                   ; set initial for PWM 512 = motor no move  
     direction = pid_Out.15
     pid_Kp =$0140                    
     pid_Ki =$0020                           
     pid_Kd =$0003                        
     pid_Ti = 8                           
     pid_I_Clamp = 1                        
     pid_Out_Clamp = 511
     preset_TMR1 = 65043  
     TMR1L = preset_TMR1.byte0
     TMR1H = preset_TMR1.byte1 
@    INT_ENABLE  TMR1_INT     ; Enable Timer 1 Interrupts     
     
     Main:                                        ;  main loop
     IF PORTA.1 = 0 then setpoint = setpoint + 1  ; Increment setpoint from taster on RA1
     select case setpoint                         ; Circular  count for Setpoint condition
     case is >= 65534                             ; Circular  count for Setpoint condition
     setpoint = 0                                 ; reset setpoint
     POSCNTH=0 ; set counter for encoder, H bit   ; reset QEI counter high byte
     POSCNTL=0 ; set counter for encoder, L bit   ; reset QEI counter low byte
     end select                                   ; End of Circular  count for Setpoint condition
     pid_Error = setpoint - position              ; Calculate PID error for PID rutine
     gosub pid                                    ; go to PID rutine
     select case direction                        ; condition determine PWM for locked antiphase  
     case 0                                       ; condition determine PWM for locked antiphase
     Duty = abs(512-pid_out)                      ; condition determine PWM for locked antiphase
     case 1                                       ; condition determine PWM for locked antiphase
     Duty = 512 + pid_Out                         ; condition determine PWM for locked antiphase
     end select                                   ; End condition determine PWM for locked antiphase
@    INT_DISABLE  TMR1_INT                        ; Prevent disable Timer1 interupt 
     CCPR1L = duty>>2                             ; MSB of duty cycle value
     CCP1CON=%00001111 | (duty<<5)                ; set PWM mode and store the 
@    INT_ENABLE  TMR1_INT                         ; Enable Timer1 interupt
     goto Main                                    ; do main loop
    
pidloop:                                          ; Interrupt for fast reading QEI and calc position
     PosHigh = POSCNTH                            ; reading QEI and calc position
     PosLow = POSCNTL                             ; reading QEI and calc position
     PosTemp = POSCNTL                            ; reading QEI and calc position
     If PosLow - PosTemp = 0 then Goto Done       ; reading QEI and calc position
     PosHigh = POSCNTH                            ; reading QEI and calc position
     PosLow = POSCNTL                             ; Done reading QEI and calc position
Done:                                             ; Final QEI calc position
     Position = 256 * POSHIGH + PosLow            ; Final QEI calc position
     TMR1L = preset_TMR1.byte0                    ; reset Timer1 to preset
     TMR1H = preset_TMR1.byte1                    ; reset Timer1 to preset
@ INT_RETURN                                      ; return from Timer1 interrupt
      end