Diff: hallsensor_software_decoder.h

Revision:

1:878564121574

Parent:

0:0532a6e10a23

Child:

2:b041de36c002

--- a/hallsensor_software_decoder.h	Wed Mar 13 06:22:03 2019 +0000
+++ b/hallsensor_software_decoder.h	Wed Mar 27 01:24:51 2019 +0000
@@ -17,25 +17,16 @@
 void init_CN();
 void CN_ITR();
 
-/*********************************************
- * for_back = 1  -> forward
- * for_back = 0  -> backward
- *********************************************/
-int for_back_1      = 0;  
-int for_back_2      = 0;  
+typedef volatile struct WheelState {
+    volatile int hallA;
+    volatile int hallB;
+    volatile unsigned short state;
+    volatile unsigned short prestate;
+    volatile int numStateChange;
+    volatile bool direction;            // true: forward, false: backward
+} WheelState_t;  
 
-int HallA_1_state   = 0;
-int HallB_1_state   = 0;
-int state_1         = 0;
-int state_1_old     = 0;
-
-int HallA_2_state   = 0;
-int HallB_2_state   = 0;
-int state_2         = 0;
-int state_2_old     = 0;
-
-int speed_count_1;
-int speed_count_2;
+WheelState_t wheelState1, wheelState2;
 
 void init_CN(){
     HallA_1.rise(&CN_ITR);
@@ -51,78 +42,90 @@
 
 void CN_ITR() {
     /* MOTOR1 */
-    HallA_1_state = HallA_1.read();
-    HallB_1_state = HallB_1.read();
+    wheelState1.hallA = HallA_1.read();
+    wheelState1.hallB = HallB_1.read();
 
-    /* state determination */
-    state_1 = (HallA_1_state << 1) + (((~HallA_1_state)&&HallB_1_state) || (HallA_1_state && (~HallB_1_state))) + 1;
+    /******************************** 
+     **  State Estimation
+     **     1. hallA = 0, 1
+     **     2. hallB = 0, 1 
+     ********************************/
+    wheelState1.state = (wheelState1.hallA << 1) + (wheelState1.hallA ^ wheelState1.hallB) + 1;
     
     
-    if(state_1 == 1){
-        if(state_1_old == 4){
-            for_back_1 = 1;
+    if(wheelState1.state == 1){
+        if(wheelState1.prestate == 4){
+            wheelState1.direction = true;
         } else if(state_1_old != 1){
-            for_back_1 = 0;
+            wheelState1.direction = false;
         }
     }
-    else if(state_1 == 4){
-        if(state_1_old == 1){
-            for_back_1 = 0;
-        } else if(state_1_old != 4) {
-            for_back_1 = 1;
+    else if(wheelState1.state == 4){
+        if(wheelState1.prestate == 1){
+            wheelState1.direction = false;
+        } else if(wheelState1.prestate != 4) {
+            wheelState1.direction = true;
         }
     }
     else {
-        if(state_1 > state_1_old){
-            for_back_1 = 1;
-        }else if(state_1 < state_1_old){
-            for_back_1 = 0;
+        if(wheelState1.state > wheelState.prestate){
+            wheelState1.direction = true;
+        }else if(wheelState1.state < wheelState.prestate){
+            wheelState1.direction = false;
         }   
     }        
     
     /* do nothing if state ain't change */
-    if(state_1_old != state_1){
-        (for_back_1)? ++speed_count_1 : --speed_count_1;
+    if(wheelState1.state != wheelState.prestate){
+        if(wheelState1.direction) { 
+            ++wheelState1.numStateChange; 
+        } else {
+            --wheelState1.numStateChange; 
+        }
     }    
     /* update previous state */
-    state_1_old = state_1; 
-    
+    wheelState1.prestate = wheelState1.state;    
     
     /* MOTOR2 */
-    HallA_2_state = HallA_2.read();
-    HallB_2_state = HallB_2.read();
+    wheelState2.hallA = HallA_2.read();
+    wheelState2.hallB = HallB_2.read();
 
     /* state determination */
-    state_2 = (HallA_2_state << 1) + (((~HallA_2_state)&&HallB_2_state) || (HallA_2_state && (~HallB_2_state))) + 1;
+    wheelState2.state = (wheelState2.hallA << 1) + (wheelState2.hallA ^ wheelState2.hallB) + 1;
     
     
-    if(state_2 == 1){
-        if(state_2_old == 4){
-            for_back_2 = 1;
-        } else if(state_2_old != 1){
-            for_back_2 = 0;
+    if(wheelState2.state == 1){
+        if(wheelState2.prestate == 4){
+            wheelState2.direction = true;
+        } else if(wheelState2.prestate != 1){
+            wheelState2.direction = false;
         }
     }
-    else if(state_2 == 4){
-        if(state_2_old == 1){
-            for_back_2 = 0;
-        } else if(state_2_old != 4){
-            for_back_2 = 1;
+    else if(wheelState2.state == 4){
+        if(wheelState2.prestate == 1){
+            wheelState2.direction = false;
+        } else if(wheelState2.prestate != 4){
+            wheelState2.direction = true;
         }
     }
     else{
-        if(state_2 > state_2_old){
-            for_back_2 = 1;
-        } else if(state_2 < state_2_old){
-            for_back_2 = 0;
+        if(wheelState2.state > wheelState2.prestate){
+            wheelState2.direction = true;
+        } else if(wheelState2.state < wheelState2.prestate){
+            wheelState2.direction = false;
         }   
-    }        
+    } 
+           
     /* do nothing if state ain't change */
-    if(state_2_old != state_2){
-        (for_back_2)? ++speed_count_2 : --speed_count_2;
+    if(wheelState2.state != wheelState2.prestate){
+        if(wheelState2.direction) { 
+            ++wheelState2.numStateChange; 
+        } else {
+            --wheelState2.numStateChange; 
+        }
     }    
     /* update previous state */
-    state_2_old = state_2; 
+    wheelState2.prestate = wheelState2.state;
 }