here is what I rewrite in incPID
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.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:
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