Diff: RTOS-Setup/src/setup.cpp

Revision:

21:b642c18eccd1

Parent:

20:b193a50a2ba3

Child:

22:ef8aa9728013

--- a/RTOS-Setup/src/setup.cpp	Sat May 03 02:55:24 2014 +0000
+++ b/RTOS-Setup/src/setup.cpp	Thu May 08 09:39:12 2014 +0000
@@ -1,3 +1,12 @@
+float KP_PITCH_STABLE = 0.0;//0.5;
+float KP_ROLL_STABLE = 0.0;//0.5;
+float KP_YAW_RATE = 8.5;
+float KP_PITCH_RATE = 8.5;//4.0;
+float KP_ROLL_RATE = 8.5;//4.0;
+
+float PID_TI_RATE = 0.0;
+float PID_TI_STABLE = 0.0;
+
 #include "setup.h"
 #include "tasks.h"
 
@@ -16,13 +25,12 @@
 //DigitalOut LED[4] = {LED1, LED2, LED3, LED4};
 
 //Kc, Ti, Td, interval
-float KP_YAW_RATE = 0.0;
-float KP_PITCH_RATE = 0.0;
-float KP_ROLL_RATE = 0.0;
+PID yawPIDrate(KP_YAW_RATE, PID_TI_RATE, 0.0, TASK2_PERIOD/1000.0);
+PID pitchPIDrate(KP_PITCH_RATE, PID_TI_RATE, 0.0, TASK2_PERIOD/1000.0);
+PID rollPIDrate(KP_ROLL_RATE, PID_TI_RATE, 0.0, TASK2_PERIOD/1000.0);
 
-PID yawPIDrate(KP_YAW_RATE, 0.0, 0.0, TASK2_PERIOD/1000.0);
-PID pitchPIDrate(KP_PITCH_RATE, 0.0, 0.0, TASK2_PERIOD/1000.0);
-PID rollPIDrate(KP_ROLL_RATE, 0.0, 0.0, TASK2_PERIOD/1000.0);
+PID pitchPIDstable(KP_PITCH_STABLE, PID_TI_STABLE, 0.0, TASK1_PERIOD/1000.0);
+PID rollPIDstable(KP_ROLL_STABLE, PID_TI_STABLE, 0.0, TASK1_PERIOD/1000.0);
 
 PwmOut ESC[4] = {p21, p22, p23, p24};
 
@@ -35,7 +43,7 @@
 {
     bool error = false;
 
-    box_demo = true;
+    //box_demo = true;
 
     if (!setup_ESC()) {
         imu.debugSerial.printf("ESC FAILED!!!\n");
@@ -43,25 +51,25 @@
     }
 
     if (setup_bt())
-        BT.printf("BT established!\n");
+        imu.debugSerial.printf("BT established!\n");
     else error = true;
 
     if (setup_PID())
-        BT.printf("PID established!\n");
+        imu.debugSerial.printf("PID established!\n");
     else error = true;
 
     if (setup_mpu6050())
-        BT.printf("MPU6050 established!\n");
+        imu.debugSerial.printf("MPU6050 established!\n");
     else error = true;
 
 #ifdef ENABLE_COMPASS
     if (setup_compass())
-        BT.printf("Compass established!\n");
+        imu.debugSerial.printf("Compass established!\n");
     else error = true;
 #endif
 
     if (setup_altimeter())
-        BT.printf("Altimeter established!\n");
+        imu.debugSerial.printf("Altimeter established!\n");
     else {
         error = true;
         imu.debugSerial.printf("ALTI FAILED\n");
@@ -105,17 +113,32 @@
 // ****************************************************************
 bool setup_PID(void)
 {
-    yawPIDrate.setInputLimits(-1100, 1100);
-    yawPIDrate.setOutputLimits(-1000, 1000);
+    pitchPIDstable.setInputLimits(-90.0, 90.0);
+    pitchPIDstable.setOutputLimits(-500, 500.0);
+    pitchPIDstable.setBias(0.0);
+    pitchPIDstable.setMode(AUTO_MODE);
+    
+    rollPIDstable.setInputLimits(-90.0, 90.0);
+    rollPIDstable.setOutputLimits(-500, 500.0);
+    rollPIDstable.setBias(0.0);
+    rollPIDstable.setMode(AUTO_MODE);
+    
+    yawPIDrate.setInputLimits(-500.0, 500.0);
+    yawPIDrate.setOutputLimits(-200.0, 200.0);
+    yawPIDrate.setBias(0.0);
     yawPIDrate.setMode(AUTO_MODE);
 
-    pitchPIDrate.setInputLimits(-1100, 1100);
-    pitchPIDrate.setOutputLimits(-1000, 1000);
+    pitchPIDrate.setInputLimits(-500.0, 500.0);
+    pitchPIDrate.setOutputLimits(-200.0, 200.0);
+    pitchPIDrate.setBias(0.0);
     pitchPIDrate.setMode(AUTO_MODE);
-
-    rollPIDrate.setInputLimits(-1100, 1100);
-    rollPIDrate.setOutputLimits(-1000, 1000);
+    
+    rollPIDrate.setInputLimits(-500.0, 500.0);
+    rollPIDrate.setOutputLimits(-200.0, 200.0);
+    rollPIDrate.setBias(0.0);
     rollPIDrate.setMode(AUTO_MODE);
+    
+   
 
     return true;
 }