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@3:a97f1d874f4e, 2012-10-04 (annotated)

Committer:
maetugr
Date:
Thu Oct 04 19:28:30 2012 +0000
Revision:
3:a97f1d874f4e
Parent:
2:93f703d2c4d7
Child:
4:e96b16ad986d
mehrere tests auf einmal als zwischenspeicher

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 0:0c4fafa398b4 4 #include "L3G4200D.h" // Gyro
maetugr 0:0c4fafa398b4 5 #include "ADXL345.h" // Acc
maetugr 1:5a64632b1eb9 6 #include "Servo.h" // Motor
maetugr 0:0c4fafa398b4 7
maetugr 1:5a64632b1eb9 8 // initialisation
maetugr 1:5a64632b1eb9 9 LED LEDs;
maetugr 0:0c4fafa398b4 10 TextLCD LCD(p5, p6, p7, p8, p9, p10, p11, TextLCD::LCD16x2); // RS, RW, E, D0, D1, D2, D3, Typ
maetugr 1:5a64632b1eb9 11 L3G4200D Gyro(p28, p27);
maetugr 0:0c4fafa398b4 12 ADXL345 Acc(p28, p27);
maetugr 3:a97f1d874f4e 13 //Servo Motor(p12);
maetugr 0:0c4fafa398b4 14
maetugr 2:93f703d2c4d7 15 Timer GlobalTimer;
maetugr 2:93f703d2c4d7 16
maetugr 2:93f703d2c4d7 17 #define PI 3.1415926535897932384626433832795
maetugr 2:93f703d2c4d7 18 #define Rad2Deg 57.295779513082320876798154814105
maetugr 2:93f703d2c4d7 19
maetugr 0:0c4fafa398b4 20 int main() {
maetugr 1:5a64632b1eb9 21 // LCD/LED init
maetugr 1:5a64632b1eb9 22 LCD.cls(); // Display löschen
maetugr 2:93f703d2c4d7 23 LCD.printf("FlyBed v0.2");
maetugr 1:5a64632b1eb9 24 LEDs.roll(2);
maetugr 3:a97f1d874f4e 25 LEDs = 15;
maetugr 0:0c4fafa398b4 26
maetugr 2:93f703d2c4d7 27 float Gyro_data[3];
maetugr 0:0c4fafa398b4 28 int Acc_data[3];
maetugr 3:a97f1d874f4e 29 //int Gyro_angle[3] = {0,0,0};
maetugr 2:93f703d2c4d7 30 unsigned long dt = 0;
maetugr 2:93f703d2c4d7 31 unsigned long time_loop = 0;
maetugr 0:0c4fafa398b4 32
maetugr 3:a97f1d874f4e 33 //Motor.initialize();
maetugr 2:93f703d2c4d7 34
maetugr 2:93f703d2c4d7 35 float angle = 0;//TEMP
maetugr 3:a97f1d874f4e 36 float bangle = 0;
maetugr 3:a97f1d874f4e 37 float drift = 0;
maetugr 2:93f703d2c4d7 38 //float drift = 0;
maetugr 2:93f703d2c4d7 39 GlobalTimer.start();
maetugr 0:0c4fafa398b4 40 while(1) {
maetugr 3:a97f1d874f4e 41
maetugr 3:a97f1d874f4e 42
maetugr 0:0c4fafa398b4 43
maetugr 0:0c4fafa398b4 44 Gyro.read(Gyro_data);
maetugr 2:93f703d2c4d7 45 Acc.read(Acc_data);
maetugr 3:a97f1d874f4e 46
maetugr 2:93f703d2c4d7 47 // Acc data angle
maetugr 3:a97f1d874f4e 48 //float Acc_abs = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2));
maetugr 2:93f703d2c4d7 49 //float Acc_angle = Rad2Deg * acos((float)Acc_data[2]/Acc_abs);
maetugr 3:a97f1d874f4e 50 float Acc_angle = Rad2Deg * atan2((float)Acc_data[1], (float)Acc_data[2]);
maetugr 2:93f703d2c4d7 51
maetugr 2:93f703d2c4d7 52 //angle = (0.98)*(angle + Gyro_data[0] * 0.1) + (0.02)*(Acc_angle);
maetugr 2:93f703d2c4d7 53
maetugr 3:a97f1d874f4e 54 float messfrequenz = 0.01;
maetugr 3:a97f1d874f4e 55 float geschwindigkeit = Gyro_data[0] - drift;
maetugr 3:a97f1d874f4e 56 //drift += (geschwindigkeit * messfrequenz * 0.3);
maetugr 3:a97f1d874f4e 57 angle += (geschwindigkeit * messfrequenz);
maetugr 3:a97f1d874f4e 58 angle += ((Acc_angle - angle) * messfrequenz * 0.1);
maetugr 2:93f703d2c4d7 59
maetugr 2:93f703d2c4d7 60 //for (int i= 0; i < 3;i++)
maetugr 2:93f703d2c4d7 61 //drift[i] += (Gyro_data[i]-drift[i])* 0.1;
maetugr 2:93f703d2c4d7 62
maetugr 2:93f703d2c4d7 63 //for (int i= 0; i < 3;i++)
maetugr 2:93f703d2c4d7 64 //Gyro_angle[i] += (Gyro_data[i]/*-drift[i]*/);
maetugr 2:93f703d2c4d7 65
maetugr 2:93f703d2c4d7 66 //dt berechnen
maetugr 2:93f703d2c4d7 67 dt = GlobalTimer.read_us() - time_loop;
maetugr 2:93f703d2c4d7 68 time_loop = GlobalTimer.read_us();
maetugr 2:93f703d2c4d7 69
maetugr 3:a97f1d874f4e 70 bangle += (Acc_angle - angle)/50 + Gyro_data[0] * 0.001; //dt/10000000.0-----------------------------------------
maetugr 2:93f703d2c4d7 71 //Gyro_angle[0] += (Gyro_data[0]) * 0.01;
maetugr 0:0c4fafa398b4 72 LCD.locate(0,0);
maetugr 3:a97f1d874f4e 73 //LCD.printf("%2.1f %2.1f %2.1f", Gyro_data[0],Gyro_data[1],Gyro_data[2]);
maetugr 3:a97f1d874f4e 74 //LCD.printf("%d %d |%2.1f ",Acc_data[1],Acc_data[2] ,Acc_angle); //roll(x) pitch(y) yaw(z)
maetugr 3:a97f1d874f4e 75 LCD.printf(" |%2.1f ",Acc_data[2]/20.0);
maetugr 3:a97f1d874f4e 76
maetugr 0:0c4fafa398b4 77 LCD.locate(1,0);
maetugr 3:a97f1d874f4e 78 //LCD.printf("%d %d %d %2.1f ", Acc_data[0],Acc_data[1],Acc_data[2]);
maetugr 3:a97f1d874f4e 79 //LCD.printf("%2.1f %2.1f %2.2f %2.1f", Acc_angle,Acc_angle,dt/10000.0, angle);
maetugr 3:a97f1d874f4e 80 LCD.printf("%2.1f %2.1f %2.1f ",Acc_angle , bangle, angle);
maetugr 2:93f703d2c4d7 81
maetugr 3:a97f1d874f4e 82 //Motor = 1000 + abs(Acc_data[1]); // Motorwert anpassen
maetugr 0:0c4fafa398b4 83
maetugr 2:93f703d2c4d7 84 //LED hin und her
maetugr 2:93f703d2c4d7 85 int ledset = 0;
maetugr 2:93f703d2c4d7 86 if (Acc_angle < 0)
maetugr 2:93f703d2c4d7 87 ledset += 1;
maetugr 2:93f703d2c4d7 88 else
maetugr 2:93f703d2c4d7 89 ledset += 8;
maetugr 2:93f703d2c4d7 90 if (angle < 0)
maetugr 2:93f703d2c4d7 91 ledset += 2;
maetugr 2:93f703d2c4d7 92 else
maetugr 2:93f703d2c4d7 93 ledset += 4;
maetugr 3:a97f1d874f4e 94 wait(0.1);
maetugr 2:93f703d2c4d7 95 LEDs = ledset;
maetugr 0:0c4fafa398b4 96
maetugr 2:93f703d2c4d7 97 //LEDs.rollnext();
maetugr 0:0c4fafa398b4 98 }
maetugr 0:0c4fafa398b4 99 }