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@7:9d4313510646, 2012-10-15 (annotated)

Committer:
maetugr
Date:
Mon Oct 15 17:23:06 2012 +0000
Revision:
7:9d4313510646
Parent:
6:179752756e9f
Child:
8:d25ecdcdbeb5
RC vorgelegt und vor Tickereinbau

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 7:9d4313510646 1 #include "mbed.h" // Standard Library
maetugr 7:9d4313510646 2 #include "LED.h" // LEDs framework for blinking ;)
maetugr 7:9d4313510646 3 #include "L3G4200D.h" // Gyro (Gyroscope)
maetugr 7:9d4313510646 4 #include "ADXL345.h" // Acc (Accelerometer)
maetugr 7:9d4313510646 5 #include "HMC5883.h" // Comp (Compass)
maetugr 7:9d4313510646 6 #include "BMP085.h" // Alt (Altitude sensor)
maetugr 7:9d4313510646 7 #include "RC_Channel.h" // RemoteControl Chnnels with PPM
maetugr 7:9d4313510646 8 #include "Servo.h" // Motor PPM
maetugr 7:9d4313510646 9 #include "PC.h" // Serial Port via USB for debugging
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 L3G4200D Gyro(p28, p27);
maetugr 5:818c0668fd2d 16 ADXL345 Acc(p28, p27);
maetugr 5:818c0668fd2d 17 HMC5883 Comp(p28, p27, GlobalTimer);
maetugr 5:818c0668fd2d 18 BMP085 Alt(p28, p27);
maetugr 7:9d4313510646 19 //Servo Motor_front(p18);
maetugr 7:9d4313510646 20 Servo Motor_left(p17);
maetugr 7:9d4313510646 21 Servo Motor_right(p19);
maetugr 7:9d4313510646 22 Servo Motor_back(p20);
maetugr 5:818c0668fd2d 23
maetugr 7:9d4313510646 24 PC 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 25
maetugr 2:93f703d2c4d7 26 #define PI 3.1415926535897932384626433832795
maetugr 2:93f703d2c4d7 27 #define Rad2Deg 57.295779513082320876798154814105
maetugr 2:93f703d2c4d7 28
maetugr 0:0c4fafa398b4 29 int main() {
maetugr 6:179752756e9f 30 pc.baud(57600);
maetugr 6:179752756e9f 31 pc.cls();
maetugr 6:179752756e9f 32 // init screen
maetugr 6:179752756e9f 33 pc.locate(10,5);
maetugr 6:179752756e9f 34 pc.printf("Flybed v0.2");
maetugr 1:5a64632b1eb9 35 LEDs.roll(2);
maetugr 5:818c0668fd2d 36
maetugr 5:818c0668fd2d 37 // variables for loop
maetugr 5:818c0668fd2d 38 float Gyro_data[3];
maetugr 5:818c0668fd2d 39 int Acc_data[3];
maetugr 5:818c0668fd2d 40 unsigned long dt = 0;
maetugr 5:818c0668fd2d 41 unsigned long time_loop = 0;
maetugr 5:818c0668fd2d 42 float angle[3] = {0,0,0};
maetugr 5:818c0668fd2d 43 float Acc_angle[2] = {0,0};
maetugr 5:818c0668fd2d 44 float Comp_angle = 0;
maetugr 5:818c0668fd2d 45 float tempangle = 0;
maetugr 0:0c4fafa398b4 46
maetugr 7:9d4313510646 47 Motor_left.initialize();
maetugr 7:9d4313510646 48 Motor_right.initialize();
maetugr 7:9d4313510646 49 Motor_back.initialize();
maetugr 5:818c0668fd2d 50 //Kompass kalibrieren --> Problem fremde Magnetfelder!
maetugr 5:818c0668fd2d 51 //Comp.AutoCalibration = 1;
maetugr 5:818c0668fd2d 52 short MagRawMin[3]= {-400, -400, -400}; //Gespeicherte Werte verwenden
maetugr 5:818c0668fd2d 53 short MagRawMax[3]= {400, 400, 400};
maetugr 5:818c0668fd2d 54 Comp.Calibrate(MagRawMin, MagRawMax);
maetugr 5:818c0668fd2d 55 //Comp.Calibrate(20);
maetugr 2:93f703d2c4d7 56
maetugr 5:818c0668fd2d 57 //Oversampling des Barometers setzen
maetugr 5:818c0668fd2d 58 Alt.oss = 0;
maetugr 4:e96b16ad986d 59
maetugr 2:93f703d2c4d7 60 GlobalTimer.start();
maetugr 0:0c4fafa398b4 61 while(1) {
maetugr 5:818c0668fd2d 62 // read data from sensors
maetugr 0:0c4fafa398b4 63 Gyro.read(Gyro_data);
maetugr 2:93f703d2c4d7 64 Acc.read(Acc_data);
maetugr 5:818c0668fd2d 65 Comp.Update();
maetugr 5:818c0668fd2d 66 Alt.Update();
maetugr 3:a97f1d874f4e 67
maetugr 5:818c0668fd2d 68 // calculate the angles for roll and pitch from accelerometer
maetugr 4:e96b16ad986d 69 Acc_angle[0] = Rad2Deg * atan2((float)Acc_data[1], (float)Acc_data[2]);
maetugr 5:818c0668fd2d 70 Acc_angle[1] = 3 -Rad2Deg * atan2((float)Acc_data[0], (float)Acc_data[2]); // TODO Offset accelerometer einstellen
maetugr 5:818c0668fd2d 71
maetugr 5:818c0668fd2d 72 //calculate angle for yaw from compass
maetugr 5:818c0668fd2d 73 Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]);
maetugr 2:93f703d2c4d7 74
maetugr 5:818c0668fd2d 75 // meassure dt
maetugr 7:9d4313510646 76 dt = GlobalTimer.read_us() - time_loop; // time in us since last loop
maetugr 7:9d4313510646 77 time_loop = GlobalTimer.read_us(); // set new time for next measurement
maetugr 2:93f703d2c4d7 78
maetugr 5:818c0668fd2d 79 // calculate angles for roll, pitch an yaw
maetugr 4:e96b16ad986d 80 angle[0] += (Acc_angle[0] - angle[0])/50 + Gyro_data[0] *dt/15000000.0;
maetugr 4:e96b16ad986d 81 angle[1] += (Acc_angle[1] - angle[1])/50 + Gyro_data[1] *dt/15000000.0;
maetugr 5:818c0668fd2d 82 angle[2] += (Comp_angle - angle[2])/50 - Gyro_data[2] *dt/15000000.0;
maetugr 5:818c0668fd2d 83 tempangle -= Gyro_data[2] *dt/15000000.0;
maetugr 3:a97f1d874f4e 84
maetugr 5:818c0668fd2d 85 // LCD output
maetugr 7:9d4313510646 86 pc.locate(10,5);
maetugr 7:9d4313510646 87 pc.printf("dt:%d %6.1fm ", dt, Alt.CalcAltitude(Alt.Pressure));
maetugr 7:9d4313510646 88 pc.locate(10,8);
maetugr 6:179752756e9f 89 pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]);
maetugr 7:9d4313510646 90 pc.locate(10,10);
maetugr 7:9d4313510646 91 pc.printf("Debug_Yaw: Comp:%6.1f Gyro:%6.1f ", Comp_angle, tempangle);
maetugr 7:9d4313510646 92 pc.locate(10,12);
maetugr 7:9d4313510646 93 pc.printf("Comp_Raw: %6.1f %6.1f %6.1f ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]);
maetugr 7:9d4313510646 94 pc.locate(10,13);
maetugr 7:9d4313510646 95 pc.printf("Comp_Mag: %6.1f %6.1f %6.1f ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]);
maetugr 2:93f703d2c4d7 96
maetugr 7:9d4313510646 97 // Read RC data
maetugr 7:9d4313510646 98 //RC[0].read() // TODO: RC daten lesen und einberechnen!
maetugr 7:9d4313510646 99
maetugr 7:9d4313510646 100 // PID Regelung
maetugr 0:0c4fafa398b4 101
maetugr 7:9d4313510646 102 // set new motor speeds
maetugr 7:9d4313510646 103 Motor_left = 1000 + 5*abs(angle[1]);
maetugr 7:9d4313510646 104 Motor_right = 1000 + 5*abs(angle[1]);
maetugr 7:9d4313510646 105 Motor_back = 1000 + 5*abs(angle[1]);
maetugr 0:0c4fafa398b4 106
maetugr 7:9d4313510646 107 LEDs.rollnext();
maetugr 7:9d4313510646 108 wait(0.1);
maetugr 0:0c4fafa398b4 109 }
maetugr 0:0c4fafa398b4 110 }