Diff: RTOS-Threads/src/Task2_Slave.cpp

Revision:

26:4a3323ee36d5

Parent:

25:a7cfe421cb4a

Child:

27:18b6580eb0b1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RTOS-Threads/src/Task2_Slave.cpp	Sat May 10 01:04:17 2014 +0000
@@ -0,0 +1,87 @@
+/* File:        Task2.cpp
+ * Author:      Trung Tin Ian HUA
+ * Date:        May 2014
+ * Purpose:     Thread2: Gyro sample and PID Control loop
+ * Settings:    200Hz
+ */
+
+#include "Task2_Slave.h"
+#include "setup.h"
+#include "PID.h"
+
+/* 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};
+
+bool counterESC = false;
+
+void Task2(void const *argument)
+{
+    imu.getRotation(&gx, &gy, &gz);
+    gyro[0] = gx + 60;
+    gyro[1] = gy - 15;
+    gyro[2] = gz + 25;
+
+    for (int i = 0; i < 3; i++)
+        gyro[i] /= (float) 32.8;
+
+    //if (counterTask1) {
+
+    switch (mode) {
+        case RATE:
+            yawPIDrate.setSetPoint(inputYPR[0]);
+            pitchPIDrate.setSetPoint(inputYPR[1]);
+            rollPIDrate.setSetPoint(inputYPR[2]);
+
+            yawPIDrate.setProcessValue(gyro[2]);
+            pitchPIDrate.setProcessValue(gyro[1]);
+            rollPIDrate.setProcessValue(gyro[0]);
+
+            adjust[0] = yawPIDrate.compute();
+            adjust[1] = pitchPIDrate.compute();
+            adjust[2] = rollPIDrate.compute();
+
+            counterTask1 = false;
+            counterESC = true;
+
+            break;
+
+        case STABLE:
+        default:
+            pitchPIDstable.setSetPoint(inputYPR[1]);
+            rollPIDstable.setSetPoint(inputYPR[2]);
+
+            pitchPIDstable.setProcessValue((int) (ypr[1] - ypr_offset[1]));
+            rollPIDstable.setProcessValue((int) (ypr[2] - ypr_offset[2]));
+
+            adjust_stable[1] = pitchPIDstable.compute();
+            adjust_stable[2] = rollPIDstable.compute();
+            adjust_stable[2] *= -1;
+
+            yawPIDrate.setSetPoint(inputYPR[0]);
+            pitchPIDrate.setSetPoint(adjust_stable[1]);
+            rollPIDrate.setSetPoint(adjust_stable[2]);
+
+            yawPIDrate.setProcessValue(gyro[2]);
+            pitchPIDrate.setProcessValue(gyro[1]);
+            rollPIDrate.setProcessValue(gyro[0]);
+
+            adjust[0] = yawPIDrate.compute();
+            adjust[1] = pitchPIDrate.compute();
+            adjust[2] = rollPIDrate.compute();
+
+            counterTask1 = false;
+            counterESC = true;
+            
+            break;
+    }
+
+    if (adjust_check)
+        BT.printf("%3.4f %3.4f %3.4f\n", adjust[0], adjust[1], adjust[2]);
+
+    if (gyro_out)
+        BT.printf("%3d %3d %3d\n", (int) gyro[0], (int) gyro[1], (int) gyro[2]);
+}