File content as of revision 6:2605aa78ef9f:

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

DigitalOut _myled(LED1);
DigitalOut _stopLed(LED2);

I2C _i2c(p28, p27);

Magneto _magneto(_i2c);
Acc _acc(_i2c);

InterruptIn _calibrateOrientation(p13);
InterruptIn _exitEmergencyStop(p14);

Ticker _logger;

Thread _xbeeTransmitter;

#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){
        _currentState.emergencyStop = true;
        printf("Inclinaison Max!!!!!\r\n");
    }
    
    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){
        _currentState.emergencyStop = true;
        printf("Orientation Max!!!!!\r\n");
    }
    
    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;
        _stopLed = !_stopLed;
    }
}

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){
            _currentState.inclinationState = nextInc;
        }
        
        if (nextDir != _currentState.directionState){
            _currentState.directionState = nextDir;
        }
        
        if (nextOr != _currentState.orientationState){
            _currentState.orientationState = nextOr;
        }
        
        if (_logOnNextLoop){
            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);
    }
}