// Capacitive rotary encoder
// v0.02 8 Jan 2012  C. Harrison
// (c) 2012 C. Harrison
// distributed under Creative Commons CC-BY-SA-compatible Open Source Ecology (OSE) License

// resources required:
//   1 16-bit timer/counter 1
//   a full output port, of which we use 4 pins for output
//     (other pins of port may be used for input, but we are greedy in order to get fast interrupt service)
//   1 analog comparator input pin

#define Mega
// using timer 1 for encoder
#define TIMERe 1
#define TCCReA TCCR1A
#define TCCReA_init 0 // CTC mode
#define TCCReB TCCR1B
#define TCCReB_init ( (1<<WGM12) | (1<<CS11) ) // clk div by 8 = 2MHz
#define TCNTe TCNT1
#define TCNTeL TCNT1L
#define OCReA OCR1A
#define OCReB OCR1B
#define ICReL ICR1L
#define TIMSKe TIMSK1
#define TIMSKe_init ( (1<<OCIE1A) | (1<<OCIE1B) ) // interrupt on compare A or B
#define TIFRe TIFR1
#define TIMERe_COMPA_vect TIMER1_COMPA_vect
#define TIMERe_COMPB_vect TIMER1_COMPB_vect
// pin and port assignments for A+, A-, B+, B- drive
#define EncDriveAp 10
#define EncDriveAn 11
#define EncDriveBp 12
#define EncDriveBn 13
#define EncDrivePort PORTB //we take over the whole port for interrupt service speed
#ifdef Mega
  #define EncDriveLSB 4
#else
  #define EncDriveLSB 2
#endif
// comparator input pins
#ifdef Mega
  #define CompareNeg 5
#else
  #define CompareNeg 7
#endif

// Carrier frequency of 10kHz, Cpu clock 16MHz, timer prescale div8 => 200 cts/cycle => 50 cts/quad step
#define CarrierPeriodNominal 42  // prototype resonance ~ 11.6kHz

// Working variables
static volatile unsigned char drivePhase;
const unsigned char PhaseData[4] = {0b1100<<EncDriveLSB, 0b1001<<EncDriveLSB,
                                    0b0011<<EncDriveLSB, 0b0110<<EncDriveLSB};
unsigned char carrierPeriod;
static volatile unsigned char capturedPhase, capturedSubPhase, capturedCount;
static unsigned char subPhase;
static int encoderCycles;

unsigned long when;
unsigned int reportInterval;

boolean entryComplete;
int value;
boolean negValue;

// Interrupt service routine for carrier timer
//  comp A & B interrupts do the same thing on alternate quadrature steps
ISR(TIMERe_COMPA_vect)
{
  // set output pins 
  EncDrivePort = PhaseData[drivePhase];
  if(TIFRe & (1<<ICF1) ) {
    capturedSubPhase = ICReL;
    capturedPhase = drivePhase;
    if(capturedSubPhase < TCNTeL) {
      capturedPhase++;
    }
    if(capturedPhase&2) {
      capturedSubPhase += 2*carrierPeriod;
    }
    TIFRe = (1<<ICF1); // reset capture flag
  }
  drivePhase = (drivePhase+1)&2; // force even phase
}
ISR(TIMERe_COMPB_vect)
{
  // set output pins 
  EncDrivePort = PhaseData[drivePhase];
  if(TIFRe & (1<<ICF1) ) {
    capturedSubPhase = ICReL;
    capturedPhase = drivePhase; 
    if(capturedSubPhase >= carrierPeriod) {
      capturedPhase++;
    } 
    if(capturedPhase&2) {
      capturedSubPhase += 2*carrierPeriod;
    }
    TIFRe = (1<<ICF1); // reset capture flag
  }
  drivePhase = (drivePhase+1)&3;
}

//Initialization.

void setup(){
  
  Serial.begin(9600);
  entryComplete = true;
  value = 0;

  pinMode(EncDriveAp,OUTPUT);
  pinMode(EncDriveAn,OUTPUT);
  pinMode(EncDriveBp,OUTPUT);
  pinMode(EncDriveBn,OUTPUT);
  
  pinMode(CompareNeg,INPUT);
  
  //debug comparator
  pinMode(26,OUTPUT);
  
  drivePhase = 0;
  carrierPeriod = CarrierPeriodNominal;
  encoderCycles = 0;
  
  // set up timer
  TCCReA = TCCReA_init;
  TCCReB = TCCReB_init;     // by clk/8
  TIMSKe = TIMSKe_init;     //Timer2 Overflow Interrupt Enable
  TCNTe = 0;
  OCReA = 2*(unsigned int)carrierPeriod-1;
  OCReB = carrierPeriod-1;
  
  // set up comparator
  ACSR = (1<<ACIC); // trigger timer capture 
  #ifdef Mega
    ACSR |= (1<<ACBG);
  #endif
  // route Bandgap reference to external pin
  ADMUX |= (1<<REFS1);
  
  when = millis()+1000;
  reportInterval = 1000;
  
  Serial.println("Initialized");

}

void loop(){
  
  int incomingByte;
  int current;

  // simple interactive command protocol: commands are of form [-]<nnnn>C
  //  where <nnnn> is a decimal number and C is a single-letter command code
  
  // check if data has been sent from the computer:
  if (Serial.available()) {
    // read the most recent byte (which will be a decimal digit or command letter):
    incomingByte = Serial.read();
    Serial.write(incomingByte); //echo
    if (incomingByte >= '0' && incomingByte <= '9') {
      if (entryComplete) {
        value = 0;
        negValue = false;
        entryComplete = false;
      }
      value = value * 10 + (incomingByte - '0')*(negValue ? -1 : 1);
    }
    else
      entryComplete = true;
    if (incomingByte == '-') {
      value = 0;
      negValue = true;
      entryComplete = false;
    }
    if (incomingByte == 'P' || incomingByte == 'p') {
      // set new period
      carrierPeriod = value;
      //while(TCNTe > (value>>1)) { }; // avoid rogue long pulse (not tested)
      OCReA = 2*(unsigned int)carrierPeriod-1;
      OCReB = carrierPeriod-1;
    }
    if (incomingByte == 'R' || incomingByte == 'r') {
      // set reporting period
      reportInterval = value;
    }
    if (incomingByte == 'Q' || incomingByte == 'q') {
      // query status
      Serial.print(encoderCycles); Serial.print("+"); Serial.print(subPhase);
      Serial.print(" = "); Serial.println((float)subPhase/(4.0*carrierPeriod)+encoderCycles);
    }
  } // endif serial available
  
  // update encoder reading
  {
    int prevSubPhase = subPhase;
    // avoid possible race with latest capture
    unsigned char quad;
    do {
      quad = capturedPhase;
      subPhase = capturedSubPhase;
    } while (quad != capturedPhase);
    // accumulate cycle counter if subPhase rolled over
    int diff = subPhase - prevSubPhase;
    if (diff > 3*carrierPeriod) {
      encoderCycles--;
    } else if (diff < -3*carrierPeriod) {
      encoderCycles++;
    }
  } 

  if(millis() > when && reportInterval > 100) {
    when = millis() + reportInterval;
    Serial.print(encoderCycles); Serial.print("+"); Serial.print(subPhase);
    Serial.print(" = "); Serial.println((float)subPhase/(4.0*carrierPeriod)+encoderCycles);
  }
}