File content as of revision 4:b6d8445792cc:

#include "mbed.h"
#include "Magneto.h"
#include "Acc.h"
#include "communication.h"
#include "xbee.h"
#include "rtos.h"

DigitalOut myled(LED1);

I2C i2c(p28, p27);

Magneto magneto(i2c);
Acc acc(i2c);

InterruptIn calibrateOrientation(p13);
InterruptIn exitEmergencyStop(p14);

Ticker logger;

Thread xbeeTransmitter;

Serial pc(USBTX, USBRX);

#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;
    uint16_t forwardOrientation;
    
    CurrentState_t() : emergencyStop(false), 
                       inclinationState(IDLE_INC), 
                       directionState(FORWARD), 
                       orientationState(IDLE_OR),
                       forwardOrientation(0) {}
};

CurrentState_t currentState;

bool calibrateOrientationOnNextLoop = false;
bool logOnNextLoop = false;

void CalibrateOrientationInterrupt(){
    calibrateOrientationOnNextLoop = true;
}

void ExitEmergencyStopInterrupt(){
    currentState.emergencyStop = false;
}

void LoggerTick(){
    logOnNextLoop = 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;
}

void XbeeCallback(char* message, int length){
    if (message[0] == STOP_COMMAND){
        currentState.emergencyStop = true;
        pc.printf("Remote Emergency Stop received\r\n");
    }
}

void MainLoop(){
    InitXbee(false, XbeeCallback, &xbeeTransmitter);
        
    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);
        
        if (logOnNextLoop){
            pc.printf("Logging data\r\n");
            char data[4] = {LOG_COMMAND, 
                            currentState.emergencyStop ? SPEED_STATE_STOP : currentState.inclinationState,
                            currentState.orientationState,
                            currentState.directionState};
            XbeeSendData(data, 4);
            logOnNextLoop = false;
        }
        
        wait(0.1);
    }
}

int main() {
    if(!magneto.TestDeviceConnection() || !acc.TestDeviceConnection()){
        pc.printf("SCRUB!!\r\n");
        return -1;
    }
    
    magneto.ActivateDevice();
    acc.ActivateDevice();
    
    calibrateOrientation.rise(CalibrateOrientationInterrupt);
    exitEmergencyStop.rise(ExitEmergencyStopInterrupt);
    
    logger.attach(LoggerTick, 2);
    
    xbeeTransmitter.start(callback(MainLoop));
    
    while (1){
        myled = !myled;
        wait(0.5);
    }
}