Diff: main.cpp

Revision:

6:2605aa78ef9f

Parent:

5:8142f455454b

diff -r 8142f455454b -r 2605aa78ef9f main.cpp
--- a/main.cpp	Mon Apr 03 18:55:55 2017 +0000
+++ b/main.cpp	Thu Apr 06 11:27:04 2017 +0000
@@ -5,23 +5,22 @@
 #include "xbee.h"
 #include "rtos.h"
 
-DigitalOut myled(LED1);
+DigitalOut _myled(LED1);
+DigitalOut _stopLed(LED2);
 
-I2C i2c(p28, p27);
+I2C _i2c(p28, p27);
 
-Magneto magneto(i2c);
-Acc acc(i2c);
+Magneto _magneto(_i2c);
+Acc _acc(_i2c);
 
-InterruptIn calibrateOrientation(p13);
-InterruptIn exitEmergencyStop(p14);
-
-Ticker logger;
+InterruptIn _calibrateOrientation(p13);
+InterruptIn _exitEmergencyStop(p14);
 
-Thread xbeeTransmitter;
+Ticker _logger;
 
-Serial pc(USBTX, USBRX);
+Thread _xbeeTransmitter;
 
-#define ABS(a) ((a)<0 ? -(a) : (a))
+#define _ABS(a) ((a)<0 ? -(a) : (a))
 
 struct CurrentState_t {
     bool emergencyStop;
@@ -37,35 +36,36 @@
                        forwardOrientation(0) {}
 };
 
-CurrentState_t currentState;
+CurrentState_t _currentState;
 
-bool calibrateOrientationOnNextLoop = false;
-bool logOnNextLoop = false;
+bool _calibrateOrientationOnNextLoop = false;
+bool _logOnNextLoop = false;
 
-void CalibrateOrientationInterrupt(){
-    calibrateOrientationOnNextLoop = true;
+void _CalibrateOrientationInterrupt(){
+    _calibrateOrientationOnNextLoop = true;
 }
 
-void ExitEmergencyStopInterrupt(){
-    currentState.emergencyStop = false;
+void _ExitEmergencyStopInterrupt(){
+    _currentState.emergencyStop = false;
 }
 
-void LoggerTick(){
-    logOnNextLoop = true;
+void _LoggerTick(){
+    _logOnNextLoop = true;
 }
 
-uint8_t GetNextInclinationState(int16_t inclination){
-    uint16_t absInc = ABS(inclination);
+uint8_t _GetNextInclinationState(int16_t inclination){
+    uint16_t absInc = _ABS(inclination);
     
-    if (absInc > 45){
-        currentState.emergencyStop = true;
+    if (absInc > 45 && !_currentState.emergencyStop){
+        _currentState.emergencyStop = true;
+        printf("Inclinaison Max!!!!!\r\n");
     }
     
-    if (currentState.emergencyStop){
+    if (_currentState.emergencyStop){
          return SPEED_STATE_IDLE;
     }
     
-    switch (currentState.inclinationState){
+    switch (_currentState.inclinationState){
     case SPEED_STATE_IDLE:
         if (absInc > 30){
             return SPEED_STATE_FAST;
@@ -88,30 +88,27 @@
         }
         break;
     }
-    return currentState.inclinationState;
+    return _currentState.inclinationState;
 }
 
-uint8_t GetNextDirectionState(int16_t inclination){
+uint8_t _GetNextDirectionState(int16_t inclination){
     return (inclination > 0 ? DIRECTION_STATE_FORWARD : DIRECTION_STATE_BACKWARD);
 }
 
-uint8_t GetNextOrientationState(int16_t orientation){
+uint8_t _GetNextOrientationState(int16_t orientation){
     // Bring the world orientation to a local reference
-    int16_t localOrientation = orientation - currentState.forwardOrientation;
+    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 (_ABS(localOrientation) > 90 && !_currentState.emergencyStop){
+        _currentState.emergencyStop = true;
+        printf("Orientation Max!!!!!\r\n");
     }
     
-    if (currentState.emergencyStop){
-         return ANGLE_STATE_STRAIGHT;
-    }
-    
-    switch(currentState.orientationState){
+    switch(_currentState.orientationState){
     case ANGLE_STATE_STRAIGHT:
         if (localOrientation < -20){
             return ANGLE_STATE_LEFT;
@@ -134,57 +131,51 @@
         }
         break;
     }
-    return currentState.orientationState;
+    return _currentState.orientationState;
 }
 
-void XbeeCallback(char* message, int length){
+void _XbeeCallback(char* message, int length){
     if (message[0] == STOP_COMMAND){
-        currentState.emergencyStop = true;
-        pc.printf("Remote Emergency Stop received\r\n");
+        _currentState.emergencyStop = true;
+        _stopLed = !_stopLed;
     }
 }
 
-void MainLoop(){
-    InitXbee(false, XbeeCallback, &xbeeTransmitter);
+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;
-        }
+        int16_t heading = _magneto.GetHeadingXY();
+        int16_t inclination = _acc.GetInclinationYZ();
         
-        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 (_calibrateOrientationOnNextLoop){
+            _currentState.forwardOrientation = heading;
+            _calibrateOrientationOnNextLoop = false;
         }
         
-        if (nextDir != currentState.directionState){
-            pc.printf("Changing Direction from %02X to %02X\r\n", currentState.directionState, nextDir);
-            currentState.directionState = nextDir;
+        uint8_t nextInc = _GetNextInclinationState(inclination);
+        uint8_t nextDir = _GetNextDirectionState(inclination);
+        uint8_t nextOr = _GetNextOrientationState(heading);
+        
+        if (nextInc != _currentState.inclinationState){
+            _currentState.inclinationState = nextInc;
         }
         
-        if (nextOr != currentState.orientationState){
-            pc.printf("Changing Orientation from %02X to %02X\r\n", currentState.orientationState, nextOr);
-            currentState.orientationState = nextOr;
+        if (nextDir != _currentState.directionState){
+            _currentState.directionState = nextDir;
         }
         
-        //pc.printf("Heading : %d, Inclination : %d\r\n", heading, inclination);
+        if (nextOr != _currentState.orientationState){
+            _currentState.orientationState = nextOr;
+        }
         
-        if (logOnNextLoop){
-            pc.printf("Logging data\r\n");
+        if (_logOnNextLoop){
             char data[4] = {LOG_COMMAND, 
-                            currentState.emergencyStop ? SPEED_STATE_STOP : currentState.inclinationState,
-                            currentState.orientationState,
-                            currentState.directionState};
+                            _currentState.emergencyStop ? SPEED_STATE_STOP : _currentState.inclinationState,
+                            _currentState.orientationState,
+                            _currentState.directionState};
             XbeeSendData(data, 4);
-            logOnNextLoop = false;
+            _logOnNextLoop = false;
         }
         
         wait(0.1);
@@ -193,25 +184,25 @@
 
 int main() {
     
-    currentState = CurrentState_t();
+    _currentState = CurrentState_t();
     
-    if(!magneto.TestDeviceConnection() || !acc.TestDeviceConnection()){
-        pc.printf("SCRUB!!\r\n");
+    if(!_magneto.TestDeviceConnection() || !_acc.TestDeviceConnection()){
+        //pc.printf("SCRUB!!\r\n");
         return -1;
     }
     
-    magneto.ActivateDevice();
-    acc.ActivateDevice();
+    _magneto.ActivateDevice();
+    _acc.ActivateDevice();
     
-    calibrateOrientation.rise(CalibrateOrientationInterrupt);
-    exitEmergencyStop.rise(ExitEmergencyStopInterrupt);
+    _calibrateOrientation.rise(_CalibrateOrientationInterrupt);
+    _exitEmergencyStop.rise(_ExitEmergencyStopInterrupt);
     
-    logger.attach(LoggerTick, 2);
+    _logger.attach(_LoggerTick, 2);
     
-    xbeeTransmitter.start(callback(MainLoop));
+    _xbeeTransmitter.start(callback(_MainLoop));
     
     while (1){
-        myled = !myled;
+        _myled = !_myled;
         wait(0.5);
     }
 }