Diff: main.cpp

Revision:

10:953afcbcebfc

Parent:

9:4e0c3936c756

Child:

11:9bf69bc6df45

--- a/main.cpp	Tue Oct 16 10:21:32 2012 +0000
+++ b/main.cpp	Wed Oct 17 08:37:08 2012 +0000
@@ -1,51 +1,51 @@
 #include "mbed.h"       // Standard Library
 #include "LED.h"        // LEDs framework for blinking ;)
+#include "PC.h"         // Serial Port via USB for debugging in TeraTerm (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe)
 #include "L3G4200D.h"   // Gyro (Gyroscope)
 #include "ADXL345.h"    // Acc (Accelerometer)
 #include "HMC5883.h"    // Comp (Compass)
 #include "BMP085.h"     // Alt (Altitude sensor)
 #include "RC_Channel.h" // RemoteControl Chnnels with PPM
 #include "Servo.h"      // Motor PPM
-#include "PC.h"         // Serial Port via USB for debugging
+#include "PID.h"        // PID Library from Aaron Berk
 
-#define PI             3.1415926535897932384626433832795
-#define Rad2Deg        57.295779513082320876798154814105
+#define PI              3.1415926535897932384626433832795
+#define Rad2Deg         57.295779513082320876798154814105
+#define RATE            0.02 // speed of Ticker/PID
 
 Timer GlobalTimer; // global time to calculate processing speed
 Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC
+PID controller(1.0, 0.0, 0.0, RATE); // test PID controller for throttle
+PID pid(1.0, 1.0, 0.0, RATE); // test PID controller for throttle
 
 // initialisation of hardware
 LED         LEDs;
-PC          pc(USBTX, USBRX, 57600);        //Achtung: Treiber auf PC fuer Serial-mbed installieren. hier herunterladen https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe
+PC          pc(USBTX, USBRX, 57600);
 L3G4200D    Gyro(p28, p27);
 ADXL345     Acc(p28, p27);
 HMC5883     Comp(p28, p27, GlobalTimer);
 BMP085      Alt(p28, p27);
 RC_Channel  RC[] = {(p10), (p11), (p12), (p13)}; // noooo p19/p20!!!!
-//Servo       Motor[] = {(p15), (p16), (p17), (p18)};
+Servo       Motor[] = {(p15), (p16), (p17), (p18)};
 
 // variables for loop
 unsigned long   dt = 0;
 unsigned long   time_loop = 0;
 float           angle[3] = {0,0,0}; // angle of calculated situation
-float           Gyro_data[3];
-int             Acc_data[3];
-float           Acc_angle[2] = {0,0};
 float           Comp_angle = 0;
 float           tempangle = 0;
+int             Motorvalue[3];
+
+float pidtester;
 
 void get_Data()
 {
     // read data from sensors
-    Gyro.read(Gyro_data);
-    Acc.read(Acc_data);
+    Gyro.read();
+    Acc.read();
     Comp.Update();
     Alt.Update();
 
-    // calculate the angles for roll and pitch from accelerometer
-    Acc_angle[0] = Rad2Deg * atan2((float)Acc_data[1], (float)Acc_data[2]);
-    Acc_angle[1] = 3 -Rad2Deg * atan2((float)Acc_data[0], (float)Acc_data[2]); // TODO Offset accelerometer einstellen
-    
     //calculate angle for yaw from compass
     Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]);
     
@@ -54,15 +54,20 @@
     time_loop = GlobalTimer.read_us();      // set new time for next measurement
     
     // calculate angles for roll, pitch an yaw
-    angle[0] += (Acc_angle[0] - angle[0])/50 + Gyro_data[0] *dt/15000000.0;
-    angle[1] += (Acc_angle[1] - angle[1])/50 + Gyro_data[1] *dt/15000000.0;
-    tempangle += (Comp_angle - tempangle)/50 - Gyro_data[2] *dt/15000000.0;
-    angle[2] -= Gyro_data[2] *dt/15000000.0; // gyro only here
+    angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt/15000000.0;
+    angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt/15000000.0;// TODO Offset accelerometer einstellen
+    tempangle += (Comp_angle - tempangle)/50 - Gyro.data[2] *dt/15000000.0;
+    angle[2] += Gyro.data[2] *dt/15000000.0; // gyro only here
     
     // Read RC data
-    //RC[0].read() // TODO: RC daten lesen und einberechnen!
-    
-    LEDs.rollnext();
+    controller.setProcessValue(RC[3 -1].read());
+    for (int j = 0; j < 4; j++)
+        Motorvalue[j] = controller.compute(); // throttle
+        
+    for (int j = 0; j < 4; j++)
+        Motor[j] = 1000 + 5*abs(angle[1]);//Motorvalue[j]; // set new motorspeeds
+    pid.setProcessValue(angle[0]);
+    pidtester = pid.compute();
 }
 
 
@@ -72,20 +77,33 @@
     pc.printf("Flybed v0.2");
     LEDs.roll(2);
     
+    controller.setInputLimits(1000.0, 2000.0);
+    controller.setOutputLimits(1000.0, 2000.0);
+    controller.setMode(AUTO_MODE);
+    
+    pid.setInputLimits(-90.0, 90.0);
+    pid.setOutputLimits(-90.0, 90.0);
+    pid.setMode(AUTO_MODE);
+    pid.setSetPoint(0.0);
+    
     // Start!
     GlobalTimer.start();
-    Datagetter.attach(&get_Data, 0.02);     // start to get data all 10ms
+    Datagetter.attach(&get_Data, RATE);     // start to get data all 10ms
     while(1) {
         pc.locate(10,5); // PC output
         pc.printf("dt:%dms  %6.1fm   ", dt/1000, Alt.CalcAltitude(Alt.Pressure));
         pc.locate(10,8);
         pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f    ", angle[0], angle[1], angle[2]);
+        pc.locate(10,9);
+        pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f    ", Acc.angle[0], Acc.angle[1], Acc.angle[2]);
         pc.locate(10,10);
         pc.printf("Debug_Yaw:  Comp:%6.1f tempangle:%6.1f  ", Comp_angle, tempangle);
         pc.locate(10,12);
         pc.printf("Comp_Raw: %6.1f %6.1f %6.1f   ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]);
         pc.locate(10,13);
         pc.printf("Comp_Mag: %6.1f %6.1f %6.1f   ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]);
-        //Motor_left = 1000 + 5*abs(angle[1]);
+        pc.locate(10,15);
+        pc.printf("pidtester: %6.1f   RC: %d %d %d %d     ", pidtester, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
+        LEDs.rollnext();
     }
 }