#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
  
  POS2CNT = 0;               // Set Current Position to 8000 does't work, reset and speed effect ???
  MAX2CNT = 0;              // Use QEI in 2x mode and 500counts by roll

  DFLT1CON = 0b0000000100000000;        // Digital filter set to off, Count Error Interrupt Disable
  DFLT2CON = 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 
  
  QEI2CONbits.QEIM = 7;           // x4 reset by MAXCNT pulse
  QEI2CONbits.POSRES = 0;         // doesn't Allow Index pulse to reset counter
  QEI2CONbits.SWPAB = 1;          // direction 
  
  //ConfigIntQEI 
    IEC3bits.QEI1IE = 1;                // Enable QEI interrupt
    IPC14bits.QEI1IP = 2;               // Interrupt priority level 2
    
    IEC4bits.QEI2IE = 1;                // Enable QEI interrupt
    IPC18bits.QEI2IP = 2;               // Interrupt priority level 2
   // extern volatile int ct_turns;
   // ct_turns = 0x8000;
   
	I2C1BRG = 0x0188; 

	//Now we will initialise the I2C peripheral for Master Mode, No Slew Rate
	//Control, and leave the peripheral switched off.
	
	I2C1CON = 0x1200;
	
	I2C1RCV = 0x0000;
	I2C1TRN = 0x0000;
	//Now we can enable the peripheral
	
	I2C1CON = 0x9200;
}


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

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




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

void __attribute__((__interrupt__)) _QEI2Interrupt(void)
{
   extern volatile int ct_turnsE2;
   // if it overflowed
     if (QEI2CONbits.UPDN ==  1){
       ct_turnsE2++;
     } else {
       ct_turnsE2--;
     }
     IFS4bits.QEI2IF = 0;    // clear the interrupt flag
}

