PMDC SERVO MOTOR WITH quadrature encoder DRIVE ?

[phoenix_1]

Here is code which can measure and put to Y variable defined as LONG position of mechanic on machine to 32bit and count up and down.
I am not good programmer but code work good.

' Pic 18F4431 @ 20MHz true XTAL
    define osc 20
    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
    ADCON0=0                           'Turn of ADC
    ANSEL0=0                           'Make Port A inputs Digital

    PortA = 0
    PortB = 0
    PortC = 0

    TRISA = %011100                    'QEI pins as inputs.
    TRISB = %00000000
    TRISC = %10000000                  'USART RX pin as input.

    QEICON=%10011000
    DFLTCON = %00111000                ' enable error filter for all capture inputs
    MAXCNTL = %11010100                'low set for maxcnt of 12500 becouse have gear 6.25:1
    MAXCNTH = %00110000                'hig set for maxcnt of 12500 becouse have gear 6.25:1
    'MAXCNTL = %00000001               'lowbyte = 207
    'MAXCNTH = %00000000               'highbyte = 7 but after math: highbyte * 256 = 1792 + 207 = 1999
    'QEICON = %00010100                'Setup QEI, 4X mode, reset on index. 
    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.
    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
    z = 0                                 
    x = 0 
    y = 0
    h = 0
    POSCNTL = 0                        ' clear lowbyte of counter for start
    POSCNTH = 0                        ' clear highbyte of counter for start 
    PIR3.2 = 0 
    CLEAR
        
    Start:
    Gosub GetPosition
    y =  (12500 * x) + position                    ' increment  total counter
    if z = 1 and h = 0 then y = y-(12500-position) ' dectement  total counter
    HSEROUT [DEC y]                                 ' debug via rs232 @ 19200
    Pause 100                                       'Wait....  
    Goto Start                                     '...and do it again.


GetPosition:
    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
    RETURN  
    END