Diff: RTOS-Threads/src/Task4.cpp

Revision:

51:04c6af4319e1

Parent:

50:8a0accb23007

--- a/RTOS-Threads/src/Task4.cpp	Mon May 19 14:16:47 2014 +0000
+++ b/RTOS-Threads/src/Task4.cpp	Mon May 19 15:30:05 2014 +0000
@@ -28,12 +28,32 @@
 // ==================
 void Task4(void const *argurment)
 {
-    if (armed) {
+    while (1) {
         if (calibration_mode) {
-            for (int i = 0; i < 4; i++)
-                ESC[i].pulsewidth_us(RCCommand[3]);
-        } else if (RCCommand[3] > 1100) {
-            if (counterESC) {
+            if (armed)
+                for (int i = 0; i < 4; i++)
+                    ESC[i].pulsewidth_us(RCCommand[3]);
+        } else if (!armed) {
+            if (ESC_check) {
+                BT.printf("Need to ARM to check ESC output!\n");
+                ESC_check = false;
+            }
+
+            for (int i = 0; i < 4; i++) {
+                ESCpower[i] = 990;
+                ESC[i].pulsewidth_us(ESCpower[i]);
+            }
+        } else if (armed) {
+            if (RCCommand[3] < 1100) {
+                for (int i = 0; i < 4; i++) {
+                    ESCpower[i] = 1000;
+                    ESC[i].pulsewidth_us(ESCpower[i]);
+                }
+            } else {
+                //PC.printf("T4\n");
+                sem_Task4.wait();
+                //PC.printf("T4 Sem\n");
+
                 for (int i = 0; i < 3; i++)
                     adjust[i] /= 2.0;
 
@@ -46,34 +66,11 @@
 
                 for (int i = 0; i < 4; i++)
                     ESC[i].pulsewidth_us(ESCpower[i]);
-
-                counterESC = false;
-            } else {
-                //stallESC++;
-                //if (stallESC > 1) {
-                //    imu.debugSerial.printf("ESC NOT UPDATED FAST ENOUGH!\n");
-                //    BT.printf("ESC NOT UPDATED FAST ENOUGH!\n");
-                //    stallESC = 0;
-                //}
-            }
-        } else {
-            for (int i = 0; i < 4; i++) {
-                ESCpower[i] = 1000;
-                ESC[i].pulsewidth_us(ESCpower[i]);
-            }
-        }
-    } else {
-        if (ESC_check) {
-            BT.printf("Need to ARM to check ESC output!\n");
-            ESC_check = false;
-        }
-
-        for (int i = 0; i < 4; i++) {
-            ESCpower[i] = 990;
-            ESC[i].pulsewidth_us(ESCpower[i]);
-        }
-    }
-}
+            } // else
+        } //else if (armed)
+        Thread::wait(TASK4_PERIOD);
+    } //while(1)
+} //Task4
 
 
 
@@ -81,11 +78,6 @@
 // ************************
 // *** Helper functions ***
 // ************************
-void Task4_ISR(void const *argument)
-{
-    //sem_Task4.release();
-}
-
 int constrainESC(float input)
 {
     if (input < 1100.0)