File content as of revision 49:731c95cd5852:

volatile float w3 = 0;                  //Angular velocity
volatile int count_i3 = 0;
 
volatile int CHA_state = 0x00;
volatile int CHB_state = 0x00;
volatile int CH_state = 0x00;
volatile int CH_state_prev = 0x00;
 
volatile float diskPosition = 0.0;  //in degrees

volatile float velocityDuty = 0.0;
 
Timer dt_I3;

int8_t updateState() {           //Convert photointerrupter inputs to a 
    return stateMap[4*I3 + 2*I2 + I1];  // rotor state. Reports 1/6 positions
}

void updateDiskPosition() {
  if (CH_state != CH_state_prev) {
    int diff = CH_state - CH_state_prev;
   
    CH_state_prev = CH_state;
    if (abs(diff) == 1 || abs(diff) == 3) {
        if (diff < 0)
            diskPosition += angularResolution;
        else
            diskPosition -= angularResolution;
    }
    else if (abs(diff) == 2) {
        if (diff < 0)
            diskPosition += 2.0f * angularResolution;
        else
            diskPosition -= 2.0f * angularResolution;
    }
 
    if (diskPosition >= 360.0f) {
      diskPosition -= 360.0f;
    } else if (diskPosition < -360.0f) {
      diskPosition += 360.0f;
    }
  }
}
 
inline void updateRelativeState() {
  CH_state = relativeStateMap[CHB_state + 2*CHA_state];
}

inline void CHA_rise() {
  CHA_state = 1;
  updateRelativeState();
  updateDiskPosition();
}

// Takes 5-6us
inline void CHA_fall() {
  CHA_state = 0;
  updateRelativeState();
  updateDiskPosition();
}
inline void CHB_rise() {
  CHB_state = 1;
  updateRelativeState();
  updateDiskPosition();
}
inline void CHB_fall() {
  CHB_state = 0;
  updateRelativeState();
  updateDiskPosition();
}

void i_edge(){
    state = updateState();
//    motorOut((state-orState+lead+6)%6, velocityDuty);
}

inline void i3rise(){
    state = updateState();
    w3 = angle/dt_I3.read();                //Calc angular velocity    
    dt_I3.reset();
 
    if (I2.read() == 1)                     //Only count revolutions if the
        count_i3++;                         // rotor spins forward
}