#include <p33FJ128MC804.h>

void QEIInit(void){

  
 //QEI configuration register 
  POS1CNT = 0;               // Set Current Position to 8000 does't work, reset and speed effect ???
  MAX1CNT = 0;              // Use QEI in 2x mode and 500counts by roll

  DFLT1CON = 0b0000000100000000;        // Digital filter set to off, Count Error Interrupt Disable

  QEI1CONbits.QEIM = 7;           // x4 reset by MAXCNT pulse
  QEI1CONbits.POSRES = 0;         // doesn't Allow Index pulse to reset counter
  QEI1CONbits.SWPAB = 1;          // direction 
 
  //ConfigIntQEI 
    IEC3bits.QEI1IE=1;                // Enable QEI interrupt
    IPC14bits.QEI1IP = 2;               // Interrupt priority level 2
    
   // extern volatile int ct_turns;
   // ct_turns = 0x8000;
    
}


unsigned short GetQEI()
{
 extern volatile int ct_turns;
 return (unsigned short) ct_turns ; //* MAXCNT + POSCNT
}




/*------------------------------------------------------------------*/
void __attribute__((__interrupt__)) _QEI1Interrupt(void)
{
   extern volatile int ct_turns;
   // if it overflowed
     if (QEI1CONbits.UPDN ==  1){
       ct_turns++;
     } else {
       ct_turns--;
     }
     IFS3bits.QEI1IF = 0;    // clear the interrupt flag
}

