File content as of revision 3:39b24d902aa7:

#include "mbed.h"
#include "Magneto.h"
#include "Acc.h"

DigitalOut myled(LED1);

I2C i2c(p28, p27);

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;
    }
    
    magneto.ActivateDevice();
    acc.ActivateDevice();
    
    wait(0.2);
    
    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);
    }
}