Diff: RTOS-Threads/src/Task3.cpp

Revision:

3:605fbcb54e75

Parent:

2:ab967d7b4346

Child:

4:01921a136f58

--- a/RTOS-Threads/src/Task3.cpp	Tue Apr 29 11:43:32 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp	Tue Apr 29 14:53:32 2014 +0000
@@ -4,17 +4,77 @@
 #include "setup.h"
 
 float ypr_offset[3];
+bool box_demo = false;
+
+Timer stallTime;
+Timer inputTime;
+
+AnalogIn rxModule[] = {p16, p17, p18, p19, p20};
+
+int rxIn[5] = {0, 0, 0, 0, 0};
+
+/* [YAW PITCH ROLL THROTTLE AUX] */
+int RCCommand[5] = {0, 0, 0, 0, 0};
 
 void Task3(void const *argument)
 {
     if (BT.readable()) {
         char data = BT.getc();
-        if (data == 'Z') {
+        if (data == 'B') {
+            box_demo = !box_demo;
+        } else if (data == 'Z') {
             ypr_offset[0] = ypr[0];
             ypr_offset[1] = ypr[1];
             ypr_offset[2] = ypr[2];
+        } else if (data == 'O') {
+            box_demo = false;
         }
     }
 
+    for (int i = 0; i < 5; i++) {
+        stallTime.reset();
+        inputTime.reset();
+
+        stallTime.start();
+        while (!rxModule[i] && (stallTime.read_us() < 1010)); // wait for high
+
+        if (stallTime.read_us() < 1050) {
+            stallTime.stop();
+            stallTime.reset();
+
+            inputTime.start();
+            stallTime.start();
+
+            while (rxModule[i] && (stallTime.read_us() < 1010)); // wait for low
+            if (stallTime.read_us() < 1050) {
+                inputTime.stop();
+                stallTime.stop();
+
+                rxIn[i] = inputTime.read_us();
+            } else {
+                stallTime.stop();
+                rxIn[i] = 0;
+            }
+        } else {
+            stallTime.stop();
+            rxIn[i] = 0;
+        }
+    }
+
+    RCCommand[0] = rxIn[0];
+    RCCommand[1] = rxIn[2];
+    RCCommand[2] = rxIn[3];
+    RCCommand[3] = 1000;//rxIn[1];
+    RCCommand[4] = rxIn[4];
+
+/*
+    for (int i = 0; i < 5; i++)
+        RCCommand[i] = 1000;
+        
+    RCCommand[0] = 0;
+    RCCommand[1] = 0;
+    RCCommand[2] = 0;
+    */
+    
     //LED[3] = !LED[3];
 }