PMDC SERVO MOTOR WITH quadrature encoder DRIVE ?

[phoenix_1]

Out of the many posts, this one attract my attention. I believe it is possible for anyone to participate.
Excellent ! I like it very much.

Yes I spend these days about 6-7 hour experimenting with PID rutine...
It is the most hard part to make PID to work.....
Soon I will post result of my expiriance...
For now regards and if any example from anyone pls post
Robert
P.S why simple when we can hard ..

[phoenix_1]

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

@ __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

[HenrikOlsson]

So, are you saying that it works now? No more "ticks" or "jumps", always smooth? What was the actual problem?

[phoenix_1]

So, are you saying that it works now? No more "ticks" or "jumps", always smooth? What was the actual problem?

To complicate calculation of position what I was make - no need for that!
I was buy "Running small motors with Pic by Harprit Singh Sandhu" in middle time and there is very good examples and sources about 18F4431 ans 16F876 - 877.After reading a couple of day's it was put me in righ direction.
That complicate calc of position was make big big proposition in interrupt and Pic was do error's .....
I must work more on that program but now it work pretty good.
Now I want remove part of reding key on RA1 and put a couple more interrupt's. One for RS232 , one for ADC reading and one for dir/step.

The best regards to all and you Henrik special.
Any suggest are welcome
/Robert

[phoenix_1]

So, are you saying that it works now? No more "ticks" or "jumps", always smooth? What was the actual problem?

Here is youtube video what I was upload

Motor move in pid control loop commanded by 16F877 which is central CPU for future Baloon printing machin which soon finished.
Machin move 8 baloon in cyrcle and print two color in same time on two printing platform controled with pneumatic via valves...etc.
On video motor only move at small distance on my desk but it was tested real and work very good.
My next project will be real PID controller to work with MACH3 software.

Thanks Henrik for PID and Darrel for DT instant interrupt and to all here.
/Robert

[phoenix_1]

Sorry comment is in my language - Serbian.
Regards to all
/Robert