Diff: RTOS-Threads/src/Task2.cpp

Revision:

10:ef5fe86f67fe

Parent:

9:371950017779

Child:

11:f9fd410c48c2

diff -r 371950017779 -r ef5fe86f67fe RTOS-Threads/src/Task2.cpp
--- a/RTOS-Threads/src/Task2.cpp	Wed Apr 30 13:49:12 2014 +0000
+++ b/RTOS-Threads/src/Task2.cpp	Thu May 01 23:50:21 2014 +0000
@@ -9,29 +9,51 @@
 int roll_adjust = 0;
 
 int16_t gx, gy, gz;
+volatile int gyro[3] = {0, 0, 0};
+
+bool counterESC = false;
 
 void Task2(void const *argument)
 {
+        imu.getRotation(&gx, &gy, &gz);
+    gyro[0] = ((float) gx / 32.8) + 2;
+    gyro[1] = ((float) gy / 32.8);
+    gyro[2] = ((float) gz / 32.8);
+    
     yawPIDrate.setSetPoint((RCCommand[0]-1500)*9/100);
     pitchPIDrate.setSetPoint((RCCommand[1]-1500)*-1*9/100);
     rollPIDrate.setSetPoint((RCCommand[2]-1500)*9/100);
 
-    imu.getRotation(&gx, &gy, &gz);
-    gx /= 32.8;
-    gx += 2;
-    gy /= 32.8;
-    gz /= 32.8;
-
-    if (gyro_out)
-        BT.printf("%4d %4d %4d\n", gx, gy, gz);
-
-    yawPIDrate.setProcessValue(gz);
-    pitchPIDrate.setProcessValue(gy);
-    rollPIDrate.setProcessValue(gx);
+    yawPIDrate.setProcessValue(gyro[2]);
+    pitchPIDrate.setProcessValue(gyro[1]);
+    rollPIDrate.setProcessValue(gyro[0]);
 
     yaw_adjust = yawPIDrate.compute();
     pitch_adjust = pitchPIDrate.compute();
     roll_adjust = rollPIDrate.compute();
 
+    /*
+    if (counterTask1) {
+    yawPIDstable.setSetPoint((RCCommand[0]-1500)*9/100);
+    pitchPIDstable.setSetPoint(RCCommand[1]-1500)*9/100);
+    rollPIDstable.setSetPoint(RCCommand[2]-1500)*9/100);
+
+    yawPIDstable.setProcessValue(ypr[0]);
+    pitchPIDstable.setProcessValue(ypr[1]);
+    rollPIDstable.setProcessValue(ypr[2]);
+
+    feed into ratePID();
+    
+    counterTask1 = false;
+    } else {
+        rate as above
+        }
+    */
+
+    counterESC = true;
+    
+    if (gyro_out)
+            BT.printf("%4d %4d %4d\n", gyro[0], gyro[1], gyro[2]);
+
     //LED[2] = !LED[2];
 }
\ No newline at end of file