Diff: main.cpp

Revision:

5:8142f455454b

Parent:

4:b6d8445792cc

Child:

6:2605aa78ef9f

--- a/main.cpp	Mon Apr 03 12:17:55 2017 +0000
+++ b/main.cpp	Mon Apr 03 18:55:55 2017 +0000
@@ -23,21 +23,17 @@
 
 #define ABS(a) ((a)<0 ? -(a) : (a))
 
-enum INCLINATION_STATE {IDLE_INC = SPEED_STATE_IDLE, SLOW = SPEED_STATE_SLOW, FAST = SPEED_STATE_FAST};
-enum DIRECTION_STATE {FORWARD = DIRECTION_STATE_FORWARD, BACKWARD = DIRECTION_STATE_BACKWARD};
-enum ORIENTATION_STATE {IDLE_OR = ANGLE_STATE_STRAIGHT, LEFT = ANGLE_STATE_LEFT, RIGHT = ANGLE_STATE_RIGHT};
-
 struct CurrentState_t {
     bool emergencyStop;
-    INCLINATION_STATE inclinationState;
-    DIRECTION_STATE directionState;
-    ORIENTATION_STATE orientationState;
+    uint8_t inclinationState;
+    uint8_t directionState;
+    uint8_t orientationState;
     uint16_t forwardOrientation;
     
     CurrentState_t() : emergencyStop(false), 
-                       inclinationState(IDLE_INC), 
-                       directionState(FORWARD), 
-                       orientationState(IDLE_OR),
+                       inclinationState(SPEED_STATE_IDLE), 
+                       directionState(DIRECTION_STATE_FORWARD), 
+                       orientationState(ANGLE_STATE_STRAIGHT),
                        forwardOrientation(0) {}
 };
 
@@ -58,7 +54,7 @@
     logOnNextLoop = true;
 }
 
-INCLINATION_STATE GetNextInclinationState(int16_t inclination){
+uint8_t GetNextInclinationState(int16_t inclination){
     uint16_t absInc = ABS(inclination);
     
     if (absInc > 45){
@@ -66,40 +62,40 @@
     }
     
     if (currentState.emergencyStop){
-         return IDLE_INC;
+         return SPEED_STATE_IDLE;
     }
     
     switch (currentState.inclinationState){
-    case IDLE_INC:
+    case SPEED_STATE_IDLE:
         if (absInc > 30){
-            return FAST;
+            return SPEED_STATE_FAST;
         } else if (absInc > 22){
-            return SLOW;
+            return SPEED_STATE_SLOW;
         }
         break;
-    case SLOW:
+    case SPEED_STATE_SLOW:
         if (absInc > 30){
-            return FAST;
+            return SPEED_STATE_FAST;
         } else if (absInc < 18){
-            return IDLE_INC;
+            return SPEED_STATE_IDLE;
         }
         break;
-    case FAST:
+    case SPEED_STATE_FAST:
         if (absInc < 18){
-            return IDLE_INC;
+            return SPEED_STATE_IDLE;
         } else if (absInc < 25){
-            return SLOW;
+            return SPEED_STATE_SLOW;
         }
         break;
     }
     return currentState.inclinationState;
 }
 
-DIRECTION_STATE GetNextDirectionState(int16_t inclination){
-    return (inclination > 0 ? FORWARD : BACKWARD);
+uint8_t GetNextDirectionState(int16_t inclination){
+    return (inclination > 0 ? DIRECTION_STATE_FORWARD : DIRECTION_STATE_BACKWARD);
 }
 
-ORIENTATION_STATE GetNextOrientationState(int16_t orientation){
+uint8_t GetNextOrientationState(int16_t orientation){
     // Bring the world orientation to a local reference
     int16_t localOrientation = orientation - currentState.forwardOrientation;
     // Be sure to have a value from 0 to 360
@@ -112,29 +108,29 @@
     }
     
     if (currentState.emergencyStop){
-         return IDLE_OR;
+         return ANGLE_STATE_STRAIGHT;
     }
     
     switch(currentState.orientationState){
-    case IDLE_OR:
+    case ANGLE_STATE_STRAIGHT:
         if (localOrientation < -20){
-            return LEFT;
+            return ANGLE_STATE_LEFT;
         } else if (localOrientation > 20){
-            return RIGHT;
+            return ANGLE_STATE_RIGHT;
         }
         break;
-    case LEFT:
+    case ANGLE_STATE_LEFT:
         if (localOrientation > 20){
-            return RIGHT;
+            return ANGLE_STATE_RIGHT;
         } else if (localOrientation > -15){
-            return IDLE_OR;
+            return ANGLE_STATE_STRAIGHT;
         }
         break;
-    case RIGHT:
+    case ANGLE_STATE_RIGHT:
         if (localOrientation < -20){
-            return LEFT;
+            return ANGLE_STATE_LEFT;
         } else if (localOrientation < 15){
-            return IDLE_OR;
+            return ANGLE_STATE_STRAIGHT;
         }
         break;
     }
@@ -160,20 +156,26 @@
             calibrateOrientationOnNextLoop = false;
         }
         
-        INCLINATION_STATE nextInc = GetNextInclinationState(inclination);
-        DIRECTION_STATE nextDir = GetNextDirectionState(inclination);
+        uint8_t nextInc = GetNextInclinationState(inclination);
+        uint8_t nextDir = GetNextDirectionState(inclination);
+        uint8_t nextOr = GetNextOrientationState(heading);
         
         if (nextInc != currentState.inclinationState){
-            pc.printf("Changing Inclination from %d to %d\r\n", currentState.inclinationState, nextInc);
+            pc.printf("Changing Inclination from %02X to %02X\r\n", currentState.inclinationState, nextInc);
             currentState.inclinationState = nextInc;
         }
         
         if (nextDir != currentState.directionState){
-            pc.printf("Changing Direction from %d to %d\r\n", currentState.directionState, nextDir);
+            pc.printf("Changing Direction from %02X to %02X\r\n", currentState.directionState, nextDir);
             currentState.directionState = nextDir;
         }
         
-        pc.printf("Heading : %d, Inclination : %d\r\n", heading, inclination);
+        if (nextOr != currentState.orientationState){
+            pc.printf("Changing Orientation from %02X to %02X\r\n", currentState.orientationState, nextOr);
+            currentState.orientationState = nextOr;
+        }
+        
+        //pc.printf("Heading : %d, Inclination : %d\r\n", heading, inclination);
         
         if (logOnNextLoop){
             pc.printf("Logging data\r\n");
@@ -190,6 +192,9 @@
 }
 
 int main() {
+    
+    currentState = CurrentState_t();
+    
     if(!magneto.TestDeviceConnection() || !acc.TestDeviceConnection()){
         pc.printf("SCRUB!!\r\n");
         return -1;