Diff: RTOS-Threads/src/Task2.cpp

Revision:

21:b642c18eccd1

Parent:

18:af657c4c3944

Child:

22:ef8aa9728013

diff -r b193a50a2ba3 -r b642c18eccd1 RTOS-Threads/src/Task2.cpp
--- a/RTOS-Threads/src/Task2.cpp	Sat May 03 02:55:24 2014 +0000
+++ b/RTOS-Threads/src/Task2.cpp	Thu May 08 09:39:12 2014 +0000
@@ -4,92 +4,65 @@
 #include "setup.h"
 #include "PID.h"
 
-int yaw_adjust = 0;
-int pitch_adjust = 0;
-int roll_adjust = 0;
+/* YPR Adjust */
+volatile float adjust[3] = {0.0, 0.0, 0.0};
+volatile float adjust_stable[3] = {0.0, 0.0, 0.0};
+
 
 int16_t gx, gy, gz;
 volatile int gyro[3] = {0, 0, 0};
+//volatile int gyro_pid[3] = {0, 0, 0};
 
 bool counterESC = false;
 
 void Task2(void const *argument)
 {
-
-    /*
-        if (counterTask1 > 1) {
-            //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]);
-
-            counterTask1 = 0;
+    imu.getRotation(&gx, &gy, &gz);
+    gyro[0] = gx + 60;
+    gyro[1] = gy - 15;
+    gyro[2] = gz + 25;
 
-            pitch_adjust_stable = pitchPIDstable.compute();
-            roll_adjust_stable = rollPIDstable.compute();
-
-            yawPIDrate.setSetPoint((RCCommand[0]-1500)*9/100);
-            pitchPIDrate.setSetPoint(pitch_adjust_stable);
-            rollPIDrate.setSetPoint(rate_adjust_stable);
+    for (int i = 0; i < 3; i++)
+        gyro[i] /= (float) 32.8;
 
-            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.setProcessValue(gyro[2]);
-            pitchPIDrate.setProcessValue(gyro[1]);
-            rollPIDrate.setProcessValue(gyro[0]);
+    //if (counterTask1) {
+    pitchPIDstable.setSetPoint(inputYPR[1]);
+    rollPIDstable.setSetPoint(inputYPR[2]);
 
-            yaw_adjust = yawPIDrate.compute();
-            pitch_adjust = pitchPIDrate.compute();
-            roll_adjust = rollPIDrate.compute();
-        } else {
-            yawPIDrate.setSetPoint(0);
-            pitchPIDrate.setSetPoint(0);
-            rollPIDrate.setSetPoint(0);
+    pitchPIDstable.setProcessValue(ypr[1] - ypr_offset[1]);
+    rollPIDstable.setProcessValue(ypr[2] - ypr_offset[2]);
 
-            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.setProcessValue(gyro[2]);
-            pitchPIDrate.setProcessValue(gyro[1]);
-            rollPIDrate.setProcessValue(gyro[0]);
+    adjust_stable[1] = pitchPIDstable.compute();
+    adjust_stable[2] = rollPIDstable.compute();
 
-            yaw_adjust = yawPIDrate.compute();
-            pitch_adjust = pitchPIDrate.compute();
-            roll_adjust = rollPIDrate.compute();
-        }
-
-    */
-
-
-    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);
+    yawPIDrate.setSetPoint(inputYPR[0]);
+    pitchPIDrate.setSetPoint(inputYPR[1]);
+    rollPIDrate.setSetPoint(inputYPR[2]);
+    //pitchPIDrate.setSetPoint(adjust_stable[1]);
+    //rollPIDrate.setSetPoint(adjust_stable[2]);
 
     yawPIDrate.setProcessValue(gyro[2]);
     pitchPIDrate.setProcessValue(gyro[1]);
     rollPIDrate.setProcessValue(gyro[0]);
 
-    yaw_adjust = yawPIDrate.compute();
-    pitch_adjust = pitchPIDrate.compute();
-    roll_adjust = rollPIDrate.compute();
+    adjust[0] = yawPIDrate.compute();
+    adjust[1] = pitchPIDrate.compute();
+    adjust[2] = rollPIDrate.compute();
+
+    counterTask1 = false;
 
     counterESC = true;
 
+    //if (adjust_check)
+    //  BT.printf("%4.4f %4.4f %4.4f\n", yaw_adjust, pitch_adjust, roll_adjust);
+
+    if (adjust_check)
+        BT.printf("%3.4f %3.4f %3.4f\n", adjust[0], adjust[1], adjust[2]);
+
     if (gyro_out)
-        BT.printf("%4d %4d %4d\n", gyro[0], gyro[1], gyro[2]);
+        BT.printf("%3d %3d %3d\n", (int) gyro[0], (int) gyro[1], (int) gyro[2]);
+    //BT.printf("%3.4f %3.4f %3.4f\n", gyro[0], gyro[1], gyro[2]);
+    //BT.printf("%6d %6d %6d\n", gx, gy, gz);
 
     //LED[2] = !LED[2];
-}
\ No newline at end of file
+}