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
Originally Posted by phoenix_1
Final test and end of these thread from me
Bookmarks