Diff: main.cpp

Revision:

4:b6d8445792cc

Parent:

3:39b24d902aa7

Child:

5:8142f455454b

--- a/main.cpp	Sun Apr 02 14:25:56 2017 +0000
+++ b/main.cpp	Mon Apr 03 12:17:55 2017 +0000
@@ -1,6 +1,9 @@
 #include "mbed.h"
 #include "Magneto.h"
 #include "Acc.h"
+#include "communication.h"
+#include "xbee.h"
+#include "rtos.h"
 
 DigitalOut myled(LED1);
 
@@ -10,14 +13,19 @@
 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 = 0, SLOW = 1, FAST = 2};
-enum DIRECTION_STATE {FORWARD = 0, BACKWARD = 1};
-enum ORIENTATION_STATE {IDLE_OR = 0, LEFT = 1, RIGHT = 2};
+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;
@@ -36,11 +44,20 @@
 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);
     
@@ -124,19 +141,16 @@
     return currentState.orientationState;
 }
 
-int main() {
-    calibrateOrientation.rise(CalibrateOrientationInterrupt);
-    
-    if(!magneto.TestDeviceConnection() || !acc.TestDeviceConnection()){
-        pc.printf("SCRUB!!\r\n");
-        return -1;
+void XbeeCallback(char* message, int length){
+    if (message[0] == STOP_COMMAND){
+        currentState.emergencyStop = true;
+        pc.printf("Remote Emergency Stop received\r\n");
     }
-    
-    magneto.ActivateDevice();
-    acc.ActivateDevice();
-    
-    wait(0.2);
-    
+}
+
+void MainLoop(){
+    InitXbee(false, XbeeCallback, &xbeeTransmitter);
+        
     while(true){
         int16_t heading = magneto.GetHeadingXY();
         int16_t inclination = acc.GetInclinationYZ();
@@ -160,6 +174,39 @@
         }
         
         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);
+    }
+}