Matthias Grob / Mbed 2 deprecated FlyBed1

NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.

Dependencies:   mbed MODI2C

Files at this revision

API Documentation at this revision

Revision 8:d25ecdcdbeb5, committed 2012-10-15

Comitter:
maetugr
Date:
Mon Oct 15 19:03:17 2012 +0000
Parent:
7:9d4313510646
Child:
9:4e0c3936c756
Commit message:
mit Ticker (aufger?umt)

Changed in this revision

LED/LED.cpp Show annotated file Show diff for this revision Revisions of this file
PC/PC.cpp Show annotated file Show diff for this revision Revisions of this file
PC/PC.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/LED/LED.cpp	Mon Oct 15 17:23:06 2012 +0000
+++ b/LED/LED.cpp	Mon Oct 15 19:03:17 2012 +0000
@@ -35,8 +35,7 @@
 void LED::rollnext() {
     if (roller >= 4)
         roller = 0;
-    else
-        roller++;
+    roller++;
     tilt(roller);
 }
 

--- a/PC/PC.cpp	Mon Oct 15 17:23:06 2012 +0000
+++ b/PC/PC.cpp	Mon Oct 15 19:03:17 2012 +0000
@@ -1,8 +1,10 @@
 #include "PC.h"
 #include "mbed.h"
 
-PC::PC(PinName tx, PinName rx) : Serial(tx, rx) 
+PC::PC(PinName tx, PinName rx, int baudrate) : Serial(tx, rx) 
 {
+    baud(baudrate);
+    cls();
 }
 
 

--- a/PC/PC.h	Mon Oct 15 17:23:06 2012 +0000
+++ b/PC/PC.h	Mon Oct 15 19:03:17 2012 +0000
@@ -6,7 +6,7 @@
 class PC : public Serial 
 {
     public:
-        PC(PinName tx, PinName rx);
+        PC(PinName tx, PinName rx, int baud);
         void cls();
         void locate(int column, int row);
 };

--- a/main.cpp	Mon Oct 15 17:23:06 2012 +0000
+++ b/main.cpp	Mon Oct 15 19:03:17 2012 +0000
@@ -8,45 +8,70 @@
 #include "Servo.h"      // Motor PPM
 #include "PC.h"         // Serial Port via USB for debugging
 
-Timer GlobalTimer;
+#define PI             3.1415926535897932384626433832795
+#define Rad2Deg        57.295779513082320876798154814105
+
+Timer GlobalTimer; // global time to calculate processing speed
+Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC
 
 // 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
 L3G4200D    Gyro(p28, p27);
 ADXL345     Acc(p28, p27);
 HMC5883     Comp(p28, p27, GlobalTimer);
 BMP085      Alt(p28, p27);
-//Servo       Motor_front(p18);
-Servo       Motor_left(p17);
-Servo       Motor_right(p19);
-Servo       Motor_back(p20);
+RC_Channel  RC[] = {(p10), (p11), (p12), (p13)}; // noooo p19/p20!!!!
+//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;
+
+void get_Data()
+{
+    // read data from sensors
+    Gyro.read(Gyro_data);
+    Acc.read(Acc_data);
+    Comp.Update();
+    Alt.Update();
 
-PC          pc(USBTX, USBRX);        //Achtung: Treiber auf PC fuer Serial-mbed installieren. hier herunterladen https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe
+    // 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]);
+    
+    // meassure dt
+    dt = GlobalTimer.read_us() - time_loop; // time in us since last loop
+    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;
+    
+    // Read RC data
+    //RC[0].read() // TODO: RC daten lesen und einberechnen!
+    
+    LEDs.rollnext();
+}
 
-#define PI             3.1415926535897932384626433832795
-#define Rad2Deg        57.295779513082320876798154814105
 
 int main() {
-    pc.baud(57600);
-    pc.cls();
     // init screen
     pc.locate(10,5);
     pc.printf("Flybed v0.2");
     LEDs.roll(2);
     
-    // variables for loop
-    float           Gyro_data[3];
-    int             Acc_data[3];
-    unsigned long   dt = 0;
-    unsigned long   time_loop = 0;
-    float angle[3] = {0,0,0};
-    float Acc_angle[2] = {0,0};
-    float Comp_angle = 0;
-    float tempangle = 0;
-    
-    Motor_left.initialize();
-    Motor_right.initialize();
-    Motor_back.initialize();
     //Kompass kalibrieren  --> Problem fremde Magnetfelder!
     //Comp.AutoCalibration = 1;
     short MagRawMin[3]= {-400, -400, -400};     //Gespeicherte Werte verwenden
@@ -54,57 +79,22 @@
     Comp.Calibrate(MagRawMin, MagRawMax);
     //Comp.Calibrate(20);
     
-    //Oversampling des Barometers setzen
-    Alt.oss = 0;
+    Alt.oss = 0; //Oversampling des Barometers setzen
     
     GlobalTimer.start();
+    Datagetter.attach(&get_Data, 0.02);     // start to get data all 10ms
     while(1) {
-        // read data from sensors
-        Gyro.read(Gyro_data);
-        Acc.read(Acc_data);
-        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]);
-        
-        // meassure dt
-        dt = GlobalTimer.read_us() - time_loop; // time in us since last loop
-        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;
-        angle[2] += (Comp_angle - angle[2])/50 - Gyro_data[2] *dt/15000000.0;
-        tempangle -= Gyro_data[2] *dt/15000000.0;
-        
-        // LCD output
+        // PC output
         pc.locate(10,5);
-        pc.printf("dt:%d %6.1fm   ", dt, Alt.CalcAltitude(Alt.Pressure));
+        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,10);
-        pc.printf("Debug_Yaw:  Comp:%6.1f Gyro:%6.1f  ", Comp_angle, tempangle);
+        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]);
-        
-        // Read RC data
-        //RC[0].read() // TODO: RC daten lesen und einberechnen!
-        
-        // PID Regelung
-        
-        // set new motor speeds
-        Motor_left = 1000 + 5*abs(angle[1]);
-        Motor_right = 1000 + 5*abs(angle[1]);
-        Motor_back = 1000 + 5*abs(angle[1]);
-        
-        LEDs.rollnext();
-        wait(0.1);
+        //Motor_left = 1000 + 5*abs(angle[1]);
     }
 }