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

main.cpp@5:818c0668fd2d, 2012-10-13 (annotated)

Committer:
maetugr
Date:
Sat Oct 13 08:59:03 2012 +0000
Revision:
5:818c0668fd2d
Parent:
4:e96b16ad986d
Child:
6:179752756e9f
?bergang von LCD auf PC-Terminal

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 1:5a64632b1eb9 1 #include "mbed.h" // Standar Library
maetugr 0:0c4fafa398b4 2 #include "LCD.h" // Display
maetugr 0:0c4fafa398b4 3 #include "LED.h" // LEDs
maetugr 5:818c0668fd2d 4 #include "L3G4200D.h" // Gyro (Gyroscope)
maetugr 5:818c0668fd2d 5 #include "ADXL345.h" // Acc (Accelerometer)
maetugr 5:818c0668fd2d 6 #include "HMC5883.h" // Comp (Compass)
maetugr 5:818c0668fd2d 7 #include "BMP085.h" // Alt (Altitude sensor)
maetugr 1:5a64632b1eb9 8 #include "Servo.h" // Motor
maetugr 5:818c0668fd2d 9 #include "terminal.h"
maetugr 0:0c4fafa398b4 10
maetugr 2:93f703d2c4d7 11 Timer GlobalTimer;
maetugr 2:93f703d2c4d7 12
maetugr 5:818c0668fd2d 13 // initialisation of hardware
maetugr 5:818c0668fd2d 14 LED LEDs;
maetugr 5:818c0668fd2d 15 TextLCD LCD(p5, p6, p7, p8, p9, p10, p11, TextLCD::LCD16x2); // RS, RW, E, D0, D1, D2, D3, Typ
maetugr 5:818c0668fd2d 16 L3G4200D Gyro(p28, p27);
maetugr 5:818c0668fd2d 17 ADXL345 Acc(p28, p27);
maetugr 5:818c0668fd2d 18 HMC5883 Comp(p28, p27, GlobalTimer);
maetugr 5:818c0668fd2d 19 BMP085 Alt(p28, p27);
maetugr 5:818c0668fd2d 20 //Servo Motor(p12);
maetugr 5:818c0668fd2d 21
maetugr 5:818c0668fd2d 22 struct terminal pc(USBTX, USBRX); //Achtung: Treiber auf PC fuer Serial-mbed installieren.
maetugr 5:818c0668fd2d 23 //hier herunterladen https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe
maetugr 5:818c0668fd2d 24
maetugr 2:93f703d2c4d7 25 #define PI 3.1415926535897932384626433832795
maetugr 2:93f703d2c4d7 26 #define Rad2Deg 57.295779513082320876798154814105
maetugr 2:93f703d2c4d7 27
maetugr 0:0c4fafa398b4 28 int main() {
maetugr 1:5a64632b1eb9 29 // LCD/LED init
maetugr 5:818c0668fd2d 30 LCD.cls(); // Display l�schen
maetugr 5:818c0668fd2d 31 LCD.printf("Flybed v0.2");
maetugr 1:5a64632b1eb9 32 LEDs.roll(2);
maetugr 5:818c0668fd2d 33
maetugr 5:818c0668fd2d 34 pc.baud(57600);
maetugr 0:0c4fafa398b4 35
maetugr 5:818c0668fd2d 36 // variables for loop
maetugr 5:818c0668fd2d 37 float Gyro_data[3];
maetugr 5:818c0668fd2d 38 int Acc_data[3];
maetugr 5:818c0668fd2d 39 unsigned long dt = 0;
maetugr 5:818c0668fd2d 40 unsigned long time_loop = 0;
maetugr 5:818c0668fd2d 41 float angle[3] = {0,0,0};
maetugr 5:818c0668fd2d 42 float Acc_angle[2] = {0,0};
maetugr 5:818c0668fd2d 43 float Comp_angle = 0;
maetugr 5:818c0668fd2d 44 float tempangle = 0;
maetugr 0:0c4fafa398b4 45
maetugr 3:a97f1d874f4e 46 //Motor.initialize();
maetugr 5:818c0668fd2d 47 //Kompass kalibrieren --> Problem fremde Magnetfelder!
maetugr 5:818c0668fd2d 48 //Comp.AutoCalibration = 1;
maetugr 5:818c0668fd2d 49 short MagRawMin[3]= {-400, -400, -400}; //Gespeicherte Werte verwenden
maetugr 5:818c0668fd2d 50 short MagRawMax[3]= {400, 400, 400};
maetugr 5:818c0668fd2d 51 Comp.Calibrate(MagRawMin, MagRawMax);
maetugr 5:818c0668fd2d 52 //Comp.Calibrate(20);
maetugr 2:93f703d2c4d7 53
maetugr 5:818c0668fd2d 54 //Oversampling des Barometers setzen
maetugr 5:818c0668fd2d 55 Alt.oss = 0;
maetugr 4:e96b16ad986d 56
maetugr 2:93f703d2c4d7 57 GlobalTimer.start();
maetugr 0:0c4fafa398b4 58 while(1) {
maetugr 5:818c0668fd2d 59 // read data from sensors
maetugr 0:0c4fafa398b4 60 Gyro.read(Gyro_data);
maetugr 2:93f703d2c4d7 61 Acc.read(Acc_data);
maetugr 5:818c0668fd2d 62 Comp.Update();
maetugr 5:818c0668fd2d 63 Alt.Update();
maetugr 3:a97f1d874f4e 64
maetugr 5:818c0668fd2d 65 // calculate the angles for roll and pitch from accelerometer
maetugr 4:e96b16ad986d 66 Acc_angle[0] = Rad2Deg * atan2((float)Acc_data[1], (float)Acc_data[2]);
maetugr 5:818c0668fd2d 67 Acc_angle[1] = 3 -Rad2Deg * atan2((float)Acc_data[0], (float)Acc_data[2]); // TODO Offset accelerometer einstellen
maetugr 5:818c0668fd2d 68
maetugr 5:818c0668fd2d 69 //calculate angle for yaw from compass
maetugr 5:818c0668fd2d 70 Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]);
maetugr 2:93f703d2c4d7 71
maetugr 5:818c0668fd2d 72 // meassure dt
maetugr 5:818c0668fd2d 73 dt = GlobalTimer.read_us() - time_loop; // Zeit in us seit letzter loop
maetugr 5:818c0668fd2d 74 time_loop = GlobalTimer.read_us(); // setze aktuelle zeit f�r n�chste messung
maetugr 2:93f703d2c4d7 75
maetugr 5:818c0668fd2d 76 // calculate angles for roll, pitch an yaw
maetugr 4:e96b16ad986d 77 angle[0] += (Acc_angle[0] - angle[0])/50 + Gyro_data[0] *dt/15000000.0;
maetugr 4:e96b16ad986d 78 angle[1] += (Acc_angle[1] - angle[1])/50 + Gyro_data[1] *dt/15000000.0;
maetugr 5:818c0668fd2d 79 angle[2] += (Comp_angle - angle[2])/50 - Gyro_data[2] *dt/15000000.0;
maetugr 5:818c0668fd2d 80 tempangle -= Gyro_data[2] *dt/15000000.0;
maetugr 3:a97f1d874f4e 81
maetugr 5:818c0668fd2d 82 int i =345;
maetugr 5:818c0668fd2d 83 // LCD output
maetugr 5:818c0668fd2d 84 pc.locate(10,5); // first line
maetugr 5:818c0668fd2d 85 pc.printf("Y:%2.1f %2.1fm ", angle[2], Alt.CalcAltitude(Alt.Pressure));
maetugr 5:818c0668fd2d 86 //LCD.printf("%2.1f %2.1f %2.1f ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]);
maetugr 5:818c0668fd2d 87 pc.locate(10,8); // second line
maetugr 5:818c0668fd2d 88 pc.printf("R:%2.1f P:%2.1f ", angle[0], angle[1]);
maetugr 5:818c0668fd2d 89 //LCD.printf("R:%2.1f P:%2.1f ", Comp_angle, tempangle);
maetugr 5:818c0668fd2d 90 //LCD.printf("%2.1f %2.1f %2.1f ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]);
maetugr 2:93f703d2c4d7 91
maetugr 5:818c0668fd2d 92 //Motor = 1000 + abs(Acc_data[1]); // set new motor speed
maetugr 0:0c4fafa398b4 93
maetugr 2:93f703d2c4d7 94 //LED hin und her
maetugr 2:93f703d2c4d7 95 int ledset = 0;
maetugr 5:818c0668fd2d 96 if (angle[0] < 0) ledset += 1; else ledset += 8;
maetugr 5:818c0668fd2d 97 if (angle[1] < 0) ledset += 2; else ledset += 4;
maetugr 2:93f703d2c4d7 98 LEDs = ledset;
maetugr 0:0c4fafa398b4 99
maetugr 2:93f703d2c4d7 100 //LEDs.rollnext();
maetugr 5:818c0668fd2d 101 //wait(0.1);
maetugr 0:0c4fafa398b4 102 }
maetugr 0:0c4fafa398b4 103 }