Diff: main.cpp

Revision:

3:39b24d902aa7

Parent:

1:3591e7df4ff4

Child:

4:b6d8445792cc

diff -r ad45959c74a9 -r 39b24d902aa7 main.cpp
--- a/main.cpp	Mon Mar 27 16:38:59 2017 +0000
+++ b/main.cpp	Sun Apr 02 14:25:56 2017 +0000
@@ -9,9 +9,124 @@
 Magneto magneto(i2c);
 Acc acc(i2c);
 
+InterruptIn calibrateOrientation(p13);
+
 Serial pc(USBTX, USBRX);
 
+#define ABS(a) ((a)<0 ? -(a) : (a))
+
+enum INCLINATION_STATE {IDLE_INC = 0, SLOW = 1, FAST = 2};
+enum DIRECTION_STATE {FORWARD = 0, BACKWARD = 1};
+enum ORIENTATION_STATE {IDLE_OR = 0, LEFT = 1, RIGHT = 2};
+
+struct CurrentState_t {
+    bool emergencyStop;
+    INCLINATION_STATE inclinationState;
+    DIRECTION_STATE directionState;
+    ORIENTATION_STATE orientationState;
+    uint16_t forwardOrientation;
+    
+    CurrentState_t() : emergencyStop(false), 
+                       inclinationState(IDLE_INC), 
+                       directionState(FORWARD), 
+                       orientationState(IDLE_OR),
+                       forwardOrientation(0) {}
+};
+
+CurrentState_t currentState;
+
+bool calibrateOrientationOnNextLoop = false;
+
+void CalibrateOrientationInterrupt(){
+    calibrateOrientationOnNextLoop = true;
+}
+
+INCLINATION_STATE GetNextInclinationState(int16_t inclination){
+    uint16_t absInc = ABS(inclination);
+    
+    if (absInc > 45){
+        currentState.emergencyStop = true;
+    }
+    
+    if (currentState.emergencyStop){
+         return IDLE_INC;
+    }
+    
+    switch (currentState.inclinationState){
+    case IDLE_INC:
+        if (absInc > 30){
+            return FAST;
+        } else if (absInc > 22){
+            return SLOW;
+        }
+        break;
+    case SLOW:
+        if (absInc > 30){
+            return FAST;
+        } else if (absInc < 18){
+            return IDLE_INC;
+        }
+        break;
+    case FAST:
+        if (absInc < 18){
+            return IDLE_INC;
+        } else if (absInc < 25){
+            return SLOW;
+        }
+        break;
+    }
+    return currentState.inclinationState;
+}
+
+DIRECTION_STATE GetNextDirectionState(int16_t inclination){
+    return (inclination > 0 ? FORWARD : BACKWARD);
+}
+
+ORIENTATION_STATE 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
+    localOrientation += localOrientation < 0 ? 360 : 0;
+    // Devide the range from 0 to 180 for the right and from -180 to 0 for the left
+    localOrientation -= localOrientation > 180 ? 360 : 0;
+    
+    if (ABS(localOrientation) > 90){
+        currentState.emergencyStop = true;
+    }
+    
+    if (currentState.emergencyStop){
+         return IDLE_OR;
+    }
+    
+    switch(currentState.orientationState){
+    case IDLE_OR:
+        if (localOrientation < -20){
+            return LEFT;
+        } else if (localOrientation > 20){
+            return RIGHT;
+        }
+        break;
+    case LEFT:
+        if (localOrientation > 20){
+            return RIGHT;
+        } else if (localOrientation > -15){
+            return IDLE_OR;
+        }
+        break;
+    case RIGHT:
+        if (localOrientation < -20){
+            return LEFT;
+        } else if (localOrientation < 15){
+            return IDLE_OR;
+        }
+        break;
+    }
+    return currentState.orientationState;
+}
+
 int main() {
+    calibrateOrientation.rise(CalibrateOrientationInterrupt);
+    
     if(!magneto.TestDeviceConnection() || !acc.TestDeviceConnection()){
         pc.printf("SCRUB!!\r\n");
         return -1;
@@ -25,6 +140,25 @@
     while(true){
         int16_t heading = magneto.GetHeadingXY();
         int16_t inclination = acc.GetInclinationYZ();
+        
+        if (calibrateOrientationOnNextLoop){
+            currentState.forwardOrientation = heading;
+            calibrateOrientationOnNextLoop = false;
+        }
+        
+        INCLINATION_STATE nextInc = GetNextInclinationState(inclination);
+        DIRECTION_STATE nextDir = GetNextDirectionState(inclination);
+        
+        if (nextInc != currentState.inclinationState){
+            pc.printf("Changing Inclination from %d to %d\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);
+            currentState.directionState = nextDir;
+        }
+        
         pc.printf("Heading : %d, Inclination : %d\r\n", heading, inclination);
         wait(0.1);
     }