Problem implement "Henrik" incPID
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:
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.
