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@11:9bf69bc6df45, 2012-10-18 (annotated)

Committer:
maetugr
Date:
Thu Oct 18 20:04:16 2012 +0000
Revision:
11:9bf69bc6df45
Parent:
10:953afcbcebfc
Child:
12:67a06c9b69d5
Kompass immernoch nicht gut, vor Kalibrierungsberechnung

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 10:953afcbcebfc 3 #include "PC.h" // Serial Port via USB for debugging in TeraTerm (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe)
maetugr 7:9d4313510646 4 #include "L3G4200D.h" // Gyro (Gyroscope)
maetugr 7:9d4313510646 5 #include "ADXL345.h" // Acc (Accelerometer)
maetugr 7:9d4313510646 6 #include "HMC5883.h" // Comp (Compass)
maetugr 7:9d4313510646 7 #include "BMP085.h" // Alt (Altitude sensor)
maetugr 7:9d4313510646 8 #include "RC_Channel.h" // RemoteControl Chnnels with PPM
maetugr 7:9d4313510646 9 #include "Servo.h" // Motor PPM
maetugr 10:953afcbcebfc 10 #include "PID.h" // PID Library from Aaron Berk
maetugr 0:0c4fafa398b4 11
maetugr 10:953afcbcebfc 12 #define PI 3.1415926535897932384626433832795
maetugr 10:953afcbcebfc 13 #define Rad2Deg 57.295779513082320876798154814105
maetugr 10:953afcbcebfc 14 #define RATE 0.02 // speed of Ticker/PID
maetugr 8:d25ecdcdbeb5 15
maetugr 8:d25ecdcdbeb5 16 Timer GlobalTimer; // global time to calculate processing speed
maetugr 8:d25ecdcdbeb5 17 Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC
maetugr 10:953afcbcebfc 18 PID controller(1.0, 0.0, 0.0, RATE); // test PID controller for throttle
maetugr 10:953afcbcebfc 19 PID pid(1.0, 1.0, 0.0, RATE); // test PID controller for throttle
maetugr 2:93f703d2c4d7 20
maetugr 5:818c0668fd2d 21 // initialisation of hardware
maetugr 5:818c0668fd2d 22 LED LEDs;
maetugr 10:953afcbcebfc 23 PC pc(USBTX, USBRX, 57600);
maetugr 5:818c0668fd2d 24 L3G4200D Gyro(p28, p27);
maetugr 5:818c0668fd2d 25 ADXL345 Acc(p28, p27);
maetugr 11:9bf69bc6df45 26 HMC5883 Comp(p28, p27);
maetugr 5:818c0668fd2d 27 BMP085 Alt(p28, p27);
maetugr 8:d25ecdcdbeb5 28 RC_Channel RC[] = {(p10), (p11), (p12), (p13)}; // noooo p19/p20!!!!
maetugr 10:953afcbcebfc 29 Servo Motor[] = {(p15), (p16), (p17), (p18)};
maetugr 8:d25ecdcdbeb5 30
maetugr 8:d25ecdcdbeb5 31 // variables for loop
maetugr 8:d25ecdcdbeb5 32 unsigned long dt = 0;
maetugr 8:d25ecdcdbeb5 33 unsigned long time_loop = 0;
maetugr 11:9bf69bc6df45 34 float angle[3] = {0,0,0}; // angle 0: x,roll / 1: y,pitch / 2: z,yaw
maetugr 8:d25ecdcdbeb5 35 float Comp_angle = 0;
maetugr 8:d25ecdcdbeb5 36 float tempangle = 0;
maetugr 10:953afcbcebfc 37 int Motorvalue[3];
maetugr 10:953afcbcebfc 38
maetugr 10:953afcbcebfc 39 float pidtester;
maetugr 8:d25ecdcdbeb5 40
maetugr 8:d25ecdcdbeb5 41 void get_Data()
maetugr 8:d25ecdcdbeb5 42 {
maetugr 8:d25ecdcdbeb5 43 // read data from sensors
maetugr 10:953afcbcebfc 44 Gyro.read();
maetugr 10:953afcbcebfc 45 Acc.read();
maetugr 11:9bf69bc6df45 46 Comp.read();
maetugr 8:d25ecdcdbeb5 47 Alt.Update();
maetugr 5:818c0668fd2d 48
maetugr 8:d25ecdcdbeb5 49 //calculate angle for yaw from compass
maetugr 11:9bf69bc6df45 50 //Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]);
maetugr 8:d25ecdcdbeb5 51
maetugr 8:d25ecdcdbeb5 52 // meassure dt
maetugr 8:d25ecdcdbeb5 53 dt = GlobalTimer.read_us() - time_loop; // time in us since last loop
maetugr 8:d25ecdcdbeb5 54 time_loop = GlobalTimer.read_us(); // set new time for next measurement
maetugr 8:d25ecdcdbeb5 55
maetugr 8:d25ecdcdbeb5 56 // calculate angles for roll, pitch an yaw
maetugr 10:953afcbcebfc 57 angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt/15000000.0;
maetugr 10:953afcbcebfc 58 angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt/15000000.0;// TODO Offset accelerometer einstellen
maetugr 10:953afcbcebfc 59 tempangle += (Comp_angle - tempangle)/50 - Gyro.data[2] *dt/15000000.0;
maetugr 10:953afcbcebfc 60 angle[2] += Gyro.data[2] *dt/15000000.0; // gyro only here
maetugr 8:d25ecdcdbeb5 61
maetugr 8:d25ecdcdbeb5 62 // Read RC data
maetugr 10:953afcbcebfc 63 controller.setProcessValue(RC[3 -1].read());
maetugr 10:953afcbcebfc 64 for (int j = 0; j < 4; j++)
maetugr 10:953afcbcebfc 65 Motorvalue[j] = controller.compute(); // throttle
maetugr 11:9bf69bc6df45 66
maetugr 10:953afcbcebfc 67 for (int j = 0; j < 4; j++)
maetugr 10:953afcbcebfc 68 Motor[j] = 1000 + 5*abs(angle[1]);//Motorvalue[j]; // set new motorspeeds
maetugr 10:953afcbcebfc 69 pid.setProcessValue(angle[0]);
maetugr 10:953afcbcebfc 70 pidtester = pid.compute();
maetugr 8:d25ecdcdbeb5 71 }
maetugr 5:818c0668fd2d 72
maetugr 2:93f703d2c4d7 73
maetugr 0:0c4fafa398b4 74 int main() {
maetugr 6:179752756e9f 75 // init screen
maetugr 6:179752756e9f 76 pc.locate(10,5);
maetugr 6:179752756e9f 77 pc.printf("Flybed v0.2");
maetugr 1:5a64632b1eb9 78 LEDs.roll(2);
maetugr 5:818c0668fd2d 79
maetugr 10:953afcbcebfc 80 controller.setInputLimits(1000.0, 2000.0);
maetugr 10:953afcbcebfc 81 controller.setOutputLimits(1000.0, 2000.0);
maetugr 10:953afcbcebfc 82 controller.setMode(AUTO_MODE);
maetugr 10:953afcbcebfc 83
maetugr 10:953afcbcebfc 84 pid.setInputLimits(-90.0, 90.0);
maetugr 10:953afcbcebfc 85 pid.setOutputLimits(-90.0, 90.0);
maetugr 10:953afcbcebfc 86 pid.setMode(AUTO_MODE);
maetugr 10:953afcbcebfc 87 pid.setSetPoint(0.0);
maetugr 10:953afcbcebfc 88
maetugr 9:4e0c3936c756 89 // Start!
maetugr 2:93f703d2c4d7 90 GlobalTimer.start();
maetugr 10:953afcbcebfc 91 Datagetter.attach(&get_Data, RATE); // start to get data all 10ms
maetugr 0:0c4fafa398b4 92 while(1) {
maetugr 9:4e0c3936c756 93 pc.locate(10,5); // PC output
maetugr 8:d25ecdcdbeb5 94 pc.printf("dt:%dms %6.1fm ", dt/1000, Alt.CalcAltitude(Alt.Pressure));
maetugr 7:9d4313510646 95 pc.locate(10,8);
maetugr 6:179752756e9f 96 pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]);
maetugr 10:953afcbcebfc 97 pc.locate(10,9);
maetugr 10:953afcbcebfc 98 pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]);
maetugr 7:9d4313510646 99 pc.locate(10,10);
maetugr 8:d25ecdcdbeb5 100 pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp_angle, tempangle);
maetugr 7:9d4313510646 101 pc.locate(10,12);
maetugr 11:9bf69bc6df45 102 pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.heading);
maetugr 7:9d4313510646 103 pc.locate(10,13);
maetugr 11:9bf69bc6df45 104 pc.printf("Comp_scale: %6.4f %6.4f %6.4f ", Comp.scale[0], Comp.scale[1], Comp.scale[2]);
maetugr 10:953afcbcebfc 105 pc.locate(10,15);
maetugr 10:953afcbcebfc 106 pc.printf("pidtester: %6.1f RC: %d %d %d %d ", pidtester, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
maetugr 11:9bf69bc6df45 107 pc.locate(10,18);
maetugr 11:9bf69bc6df45 108 pc.printf("Max: %d %d %d ", Comp.Max[0], Comp.Max[1], Comp.Max[2]);
maetugr 11:9bf69bc6df45 109 pc.locate(10,19);
maetugr 11:9bf69bc6df45 110 pc.printf("Min: %d %d %d ", Comp.Min[0], Comp.Min[1], Comp.Min[2]);
maetugr 10:953afcbcebfc 111 LEDs.rollnext();
maetugr 0:0c4fafa398b4 112 }
maetugr 0:0c4fafa398b4 113 }