here is what I rewrite in incPID

Code:

'*******************************************************************************
'*******************************************************************************
'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 :

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

## Bookmarks