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@6:179752756e9f, 2012-10-13 (annotated)

Committer:
maetugr
Date:
Sat Oct 13 11:12:22 2012 +0000
Revision:
6:179752756e9f
Parent:
5:818c0668fd2d
Child:
7:9d4313510646
vor 4 Motortest

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