Diff: photointerruptRoutines.h

Revision:

59:fcaef23bbaaf

Parent:

53:c703e0bf5e87

Child:

60:f171b7006c73

--- a/photointerruptRoutines.h	Fri Mar 24 18:31:41 2017 +0000
+++ b/photointerruptRoutines.h	Fri Mar 24 18:54:18 2017 +0000
@@ -11,69 +11,77 @@
 volatile float velocityDuty = 0.0;
  
 Timer dt_I3;
+Mutex mutex;
 
 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];
-//}
+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) {
+        mutex.lock();
+        if (diff < 0)
+            diskPosition += angularResolution;
+        else
+            diskPosition -= angularResolution;
+        mutex.unlock();
+    }
+    else if (abs(diff) == 2) {
+        mutex.lock();
+        if (diff < 0)
+            diskPosition += 2.0f * angularResolution;
+        else
+            diskPosition -= 2.0f * angularResolution;
+        mutex.unlock();
+    }
+    mutex.lock();
+    if (diskPosition >= 360.0f) {
+      diskPosition -= 360.0f;
+    } else if (diskPosition < -360.0f) {
+      diskPosition += 360.0f;
+    }
+    mutex.unlock();
+  }
+}
+ 
+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();
-//}
+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(){
+    mutex.lock();
     state = updateState();
+    mutex.unlock();
     motorOut((state-orState+lead+6)%6, velocityDuty);
 }
-Mutex mutex;
+
 void i3rise(){
     mutex.lock();
     w3 = 1/dt_I3.read();                //Calc angular velocity 
@@ -86,6 +94,8 @@
         count_i3++;                         // rotor spins forward
         mutex.unlock();
     }
+    mutex.lock();
     state = updateState();
+    mutex.unlock();
     motorOut((state-orState+lead+6)%6, velocityDuty);
 }
\ No newline at end of file