Diff: photointerruptRoutines.h

Revision:

49:731c95cd5852

Child:

50:c0c299bf54a6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/photointerruptRoutines.h	Fri Mar 24 16:57:35 2017 +0000
@@ -0,0 +1,84 @@
+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
+}
\ No newline at end of file