Diff: RTOS-Threads/src/Task4.cpp

Revision:

21:b642c18eccd1

Parent:

19:bd88749c8db4

Child:

22:ef8aa9728013

--- a/RTOS-Threads/src/Task4.cpp	Sat May 03 02:55:24 2014 +0000
+++ b/RTOS-Threads/src/Task4.cpp	Thu May 08 09:39:12 2014 +0000
@@ -8,49 +8,69 @@
 int stallESC = 0;
 
 bool armed = false;
+bool ESC_check = false;
+bool calibration_mode = false;
 
 void Task4(void const *argurment)
 {
     if (armed) {
-        if (counterESC) {
-            /* [YAW PITCH ROLL THROTTLE AUX] */
-            ESCpower[0] = (RCCommand[3]*9/10) + (pitch_adjust / 2) - (roll_adjust / 2) + (yaw_adjust / 2);
-            ESCpower[1] = (RCCommand[3]*9/10) + (pitch_adjust / 2) + (roll_adjust / 2) - (yaw_adjust / 2);
-            ESCpower[2] = (RCCommand[3]*9/10) - (pitch_adjust / 2) + (roll_adjust / 2) + (yaw_adjust / 2);
-            ESCpower[3] = (RCCommand[3]*9/10) - (pitch_adjust / 2) - (roll_adjust / 2) - (yaw_adjust / 2);
-            
+        if (calibration_mode) {
             for (int i = 0; i < 4; i++)
-                ESC[i] = constrainESC(ESC[i]);
+                ESC[i].pulsewidth_us(RCCommand[3]);
+        } else if (RCCommand[3] > 1150) {
+            if (counterESC) {
+                /* [YAW PITCH ROLL THROTTLE AUX] */
+                
+                for (int i = 0; i < 3; i++)
+                    adjust[i] /= 2.0;
+                
+                ESCpower[0] = constrainESC((RCCommand[3]*9/10) + (adjust[1]) + (adjust[2]) - adjust[0]);
+                ESCpower[1] = constrainESC((RCCommand[3]*9/10) + (adjust[1]) - (adjust[2]) + adjust[0]);
+                ESCpower[2] = constrainESC((RCCommand[3]*9/10) - (adjust[1]) - (adjust[2]) - adjust[0]);
+                ESCpower[3] = constrainESC((RCCommand[3]*9/10) - (adjust[1]) + (adjust[2]) + adjust[0]);
 
-            // if (ESC_check)
-            //BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);
+                for (int i = 0; i < 4; i++)
+                    ESC[i].pulsewidth_us(ESCpower[i]);
 
-            for (int i = 0; i < 4; i++)
-                ESC[i].pulsewidth_us(ESCpower[i]);
+                counterESC = false;
+            } else {
+                stallESC++;
 
-            counterESC = false;
+                if (stallESC > 1) {
+                    imu.debugSerial.printf("ESC NOT UPDATED FAST ENOUGH!\n");
+                    stallESC = 0;
+                }
+            }
         } else {
-            stallESC++;
-
-            if (stallESC > 1) {
-                imu.debugSerial.printf("ESC NOT UPDATED FAST ENOUGH!\n");
-                stallESC = 0;
+            for (int i = 0; i < 4; i++) {
+                ESCpower[i] = 980;
+                ESC[i].pulsewidth_us(ESCpower[i]);
             }
         }
     } else {
-        for (int i = 0; i < 4; i++)
+        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] = 980;
             ESC[i].pulsewidth_us(ESCpower[i]);
+        }
     }
 
+    if (ESC_check)
+        BT.printf("%4d %4d %4d %4d\n", ESCpower[0], ESCpower[1], ESCpower[2], ESCpower[3]);
+
     //LED[4] = !LED[4];
 }
 
-int constrainESC(int input)
+int constrainESC(float input)
 {
-    if (input < 1000)
-        return 1000;
-    else if (input > 2000)
+    if (input < 1150.0)
+        return 1150;
+    else if (input > 2000.0)
         return 2000;
     else
-        return input;
+        return (int) input;
 }