Diff: RTOS-Setup/src/setup.cpp

Revision:

3:605fbcb54e75

Parent:

2:ab967d7b4346

Child:

4:01921a136f58

--- a/RTOS-Setup/src/setup.cpp	Tue Apr 29 11:43:32 2014 +0000
+++ b/RTOS-Setup/src/setup.cpp	Tue Apr 29 14:53:32 2014 +0000
@@ -1,46 +1,78 @@
 #include "setup.h"
 #include "tasks.h"
 
+#define KP_YAW_RATE     1.0
+#define KP_PITCH_RATE   1.0
+#define KP_ROLL_RATE    1.0
+
 Serial BT(p13, p14);
 DigitalOut BT_CMD(p12);
 
 MPU6050 imu(p9, p10);
 
-DigitalOut LED[] = {LED1, LED2, LED3, LED4};
+//DigitalOut LED[4] = {LED1, LED2, LED3, LED4};
+
+//Kc, Ti, Td, interval
+PID yawPIDrate(KP_YAW_RATE, 0.0, 0.0, TASK2_PERIOD);
+PID pitchPIDrate(KP_PITCH_RATE, 0.0, 0.0, TASK2_PERIOD);
+PID rollPIDrate(KP_ROLL_RATE, 0.0, 0.0, TASK2_PERIOD);
+
+PwmOut ESC[4] = {p21, p22, p23, p24};
+
+
 
-PwmOut ESC1(p21);
-PwmOut ESC2(p22);
-PwmOut ESC3(p23);
-PwmOut ESC4(p24);
+// ================================================================
+// === MAIN SETUP ROUTINE ===
+// ================================================================
+bool setupALLdevices(void)
+{
+    bool error = false;
+
+    if (!setup_ESC()) {
+        imu.debugSerial.printf("ESC FAILED!!!\n");
+        error = true;
+    }
 
-bool imu_available = false;
+    /*
+    if (!setup_led())
+        error = true;
+    */
+
+    if (setup_bt())
+        BT.printf("BT established!\n");
+    else error = true;
+
+    if (setup_PID())
+        BT.printf("PID established!\n");
+    else error = true;
 
-// MPU control/status vars
-bool dmpReady = false; // set true if DMP init was successful
-uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
-uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
+    if (setup_mpu6050())
+        BT.printf("MPU6050 established!\n");
+    else error = true;
+
+    return error;
+}
 
+
+
+
+// ****************************************************************
+// === ESC SETUP ROUTINE ===
+// ****************************************************************
 bool setup_ESC(void)
 {
-    ESC1.period_us(ESC_PERIOD_US);
-    ESC2.period_us(ESC_PERIOD_US);
-    ESC3.period_us(ESC_PERIOD_US);
-    ESC4.period_us(ESC_PERIOD_US);
+    for (int i = 0; i < 4; i++)
+        ESC[i].period_us(ESC_PERIOD_US);
 
-    ESC1.pulsewidth_us(0);
-    ESC2.pulsewidth_us(0);
-    ESC3.pulsewidth_us(0);
-    ESC3.pulsewidth_us(0);
-    
+    for (int i = 0; i < 4; i++)
+        ESC[i].pulsewidth_us(0);
+
     return true;
 }
 
-bool setup_led(void)
-{
-    ledReset();
-    return true;
-}
-
+// ****************************************************************
+// === BLUETOOTH SETUP ROUTINE ===
+// ****************************************************************
 bool setup_bt(void)
 {
     BT.baud(115200);
@@ -49,6 +81,35 @@
     return true;
 }
 
+// ****************************************************************
+// === PID SETUP ROUTINE ===
+// ****************************************************************
+bool setup_PID(void)
+{
+    yawPIDrate.setInputLimits(-2720, 2720);
+    yawPIDrate.setOutputLimits(-500, 500);
+    yawPIDrate.setMode(AUTO_MODE);
+
+    pitchPIDrate.setInputLimits(-1720, 1720);
+    pitchPIDrate.setOutputLimits(-500, 500);
+    pitchPIDrate.setMode(AUTO_MODE);
+
+    rollPIDrate.setInputLimits(-2720, 2720);
+    rollPIDrate.setOutputLimits(-500, 500);
+    rollPIDrate.setMode(AUTO_MODE);
+    return true;
+}
+
+// ****************************************************************
+// === MPU6050 SETUP ROUTINE ===
+// ****************************************************************
+bool imu_available = false;
+
+/*/ MPU control/status variables: */
+bool dmpReady = false;          // set true if DMP init was successful
+uint8_t devStatus;              // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;            // expected DMP packet size (default is 42 bytes)
+
 bool setup_mpu6050(void)
 {
     imu.reset();
@@ -64,13 +125,11 @@
     if (imu_available) {
         devStatus = imu.dmpInitialize();
 
-        /*
-         // supply your own gyro offsets here, scaled for min sensitivity
-        mpu.setXGyroOffset(-61);
-        mpu.setYGyroOffset(-127);
-        mpu.setZGyroOffset(19);
-        mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
-        */
+        // supply your own gyro offsets here, scaled for min sensitivity
+        imu.setXGyroOffset(55);
+        imu.setYGyroOffset(3);
+        imu.setZGyroOffset(-2);
+        //mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
 
         if(!devStatus) {
             imu.setDMPEnabled(true);