File content as of revision 5:8142f455454b:

#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))

struct CurrentState_t {
    bool emergencyStop;
    uint8_t inclinationState;
    uint8_t directionState;
    uint8_t orientationState;
    uint16_t forwardOrientation;
    
    CurrentState_t() : emergencyStop(false), 
                       inclinationState(SPEED_STATE_IDLE), 
                       directionState(DIRECTION_STATE_FORWARD), 
                       orientationState(ANGLE_STATE_STRAIGHT),
                       forwardOrientation(0) {}
};

CurrentState_t currentState;

bool calibrateOrientationOnNextLoop = false;
bool logOnNextLoop = false;

void CalibrateOrientationInterrupt(){
    calibrateOrientationOnNextLoop = true;
}

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

void LoggerTick(){
    logOnNextLoop = true;
}

uint8_t GetNextInclinationState(int16_t inclination){
    uint16_t absInc = ABS(inclination);
    
    if (absInc > 45){
        currentState.emergencyStop = true;
    }
    
    if (currentState.emergencyStop){
         return SPEED_STATE_IDLE;
    }
    
    switch (currentState.inclinationState){
    case SPEED_STATE_IDLE:
        if (absInc > 30){
            return SPEED_STATE_FAST;
        } else if (absInc > 22){
            return SPEED_STATE_SLOW;
        }
        break;
    case SPEED_STATE_SLOW:
        if (absInc > 30){
            return SPEED_STATE_FAST;
        } else if (absInc < 18){
            return SPEED_STATE_IDLE;
        }
        break;
    case SPEED_STATE_FAST:
        if (absInc < 18){
            return SPEED_STATE_IDLE;
        } else if (absInc < 25){
            return SPEED_STATE_SLOW;
        }
        break;
    }
    return currentState.inclinationState;
}

uint8_t GetNextDirectionState(int16_t inclination){
    return (inclination > 0 ? DIRECTION_STATE_FORWARD : DIRECTION_STATE_BACKWARD);
}

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
    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 ANGLE_STATE_STRAIGHT;
    }
    
    switch(currentState.orientationState){
    case ANGLE_STATE_STRAIGHT:
        if (localOrientation < -20){
            return ANGLE_STATE_LEFT;
        } else if (localOrientation > 20){
            return ANGLE_STATE_RIGHT;
        }
        break;
    case ANGLE_STATE_LEFT:
        if (localOrientation > 20){
            return ANGLE_STATE_RIGHT;
        } else if (localOrientation > -15){
            return ANGLE_STATE_STRAIGHT;
        }
        break;
    case ANGLE_STATE_RIGHT:
        if (localOrientation < -20){
            return ANGLE_STATE_LEFT;
        } else if (localOrientation < 15){
            return ANGLE_STATE_STRAIGHT;
        }
        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;
        }
        
        uint8_t nextInc = GetNextInclinationState(inclination);
        uint8_t nextDir = GetNextDirectionState(inclination);
        uint8_t nextOr = GetNextOrientationState(heading);
        
        if (nextInc != currentState.inclinationState){
            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 %02X to %02X\r\n", currentState.directionState, nextDir);
            currentState.directionState = nextDir;
        }
        
        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");
            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() {
    
    currentState = CurrentState_t();
    
    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);
    }
}