File content as of revision 57:b1b4e1379613:

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);
}
Mutex mutex;
void i3rise(){
    mutex.lock();
    w3 = 1/dt_I3.read();                //Calc angular velocity 
    mutex.unlock();
       
    dt_I3.reset();
 
    if (I2.read() == 1){                     //Only count revolutions if the
        mutex.lock();
        count_i3++;                         // rotor spins forward
        mutex.unlock();
    }
    state = updateState();
    motorOut((state-orState+lead+6)%6, velocityDuty);
}

void startInterrupts() {
    I1.rise(&i_edge);
    I1.fall(&i_edge);
    I2.rise(&i_edge);
    I2.fall(&i_edge);
    I3.rise(&i3rise);
    I3.fall(&i_edge);
}