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.
main.cpp@4:e96b16ad986d, 2012-10-05 (annotated)
- Committer:
- maetugr
- Date:
- Fri Oct 05 14:05:10 2012 +0000
- Revision:
- 4:e96b16ad986d
- Parent:
- 3:a97f1d874f4e
- Child:
- 5:818c0668fd2d
alle winkel zufriedenstellend ausgerechnet, pitch hat noch offset ins positive (vor l?schen von versuchen in kommentaren)
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:e96b16ad986d | 35 | float angle[3] = {0,0,0}; |
maetugr | 4:e96b16ad986d | 36 | float Acc_angle[2] = {0,0}; |
maetugr | 4:e96b16ad986d | 37 | |
maetugr | 2:93f703d2c4d7 | 38 | GlobalTimer.start(); |
maetugr | 0:0c4fafa398b4 | 39 | while(1) { |
maetugr | 0:0c4fafa398b4 | 40 | Gyro.read(Gyro_data); |
maetugr | 2:93f703d2c4d7 | 41 | Acc.read(Acc_data); |
maetugr | 3:a97f1d874f4e | 42 | |
maetugr | 2:93f703d2c4d7 | 43 | // Acc data angle |
maetugr | 3:a97f1d874f4e | 44 | //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 | 45 | //float Acc_angle = Rad2Deg * acos((float)Acc_data[2]/Acc_abs); |
maetugr | 4:e96b16ad986d | 46 | Acc_angle[0] = Rad2Deg * atan2((float)Acc_data[1], (float)Acc_data[2]); |
maetugr | 4:e96b16ad986d | 47 | Acc_angle[1] = -Rad2Deg * atan2((float)Acc_data[0], (float)Acc_data[2]); |
maetugr | 2:93f703d2c4d7 | 48 | |
maetugr | 2:93f703d2c4d7 | 49 | //dt berechnen |
maetugr | 2:93f703d2c4d7 | 50 | dt = GlobalTimer.read_us() - time_loop; |
maetugr | 2:93f703d2c4d7 | 51 | time_loop = GlobalTimer.read_us(); |
maetugr | 2:93f703d2c4d7 | 52 | |
maetugr | 4:e96b16ad986d | 53 | angle[0] += (Acc_angle[0] - angle[0])/50 + Gyro_data[0] *dt/15000000.0; |
maetugr | 4:e96b16ad986d | 54 | angle[1] += (Acc_angle[1] - angle[1])/50 + Gyro_data[1] *dt/15000000.0; |
maetugr | 4:e96b16ad986d | 55 | angle[2] += /*(Acc_angle[1] - angle[1])/50 +*/ Gyro_data[2] *dt/15000000.0; |
maetugr | 2:93f703d2c4d7 | 56 | //Gyro_angle[0] += (Gyro_data[0]) * 0.01; |
maetugr | 0:0c4fafa398b4 | 57 | LCD.locate(0,0); |
maetugr | 3:a97f1d874f4e | 58 | //LCD.printf("%2.1f %2.1f %2.1f", Gyro_data[0],Gyro_data[1],Gyro_data[2]); |
maetugr | 3:a97f1d874f4e | 59 | //LCD.printf("%d %d |%2.1f ",Acc_data[1],Acc_data[2] ,Acc_angle); //roll(x) pitch(y) yaw(z) |
maetugr | 3:a97f1d874f4e | 60 | LCD.printf(" |%2.1f ",Acc_data[2]/20.0); |
maetugr | 3:a97f1d874f4e | 61 | |
maetugr | 0:0c4fafa398b4 | 62 | LCD.locate(1,0); |
maetugr | 3:a97f1d874f4e | 63 | //LCD.printf("%d %d %d %2.1f ", Acc_data[0],Acc_data[1],Acc_data[2]); |
maetugr | 3:a97f1d874f4e | 64 | //LCD.printf("%2.1f %2.1f %2.2f %2.1f", Acc_angle,Acc_angle,dt/10000.0, angle); |
maetugr | 4:e96b16ad986d | 65 | LCD.printf("%2.1f %2.1f %2.1f ", angle[0], angle[1], angle[2]); |
maetugr | 2:93f703d2c4d7 | 66 | |
maetugr | 3:a97f1d874f4e | 67 | //Motor = 1000 + abs(Acc_data[1]); // Motorwert anpassen |
maetugr | 0:0c4fafa398b4 | 68 | |
maetugr | 2:93f703d2c4d7 | 69 | //LED hin und her |
maetugr | 2:93f703d2c4d7 | 70 | int ledset = 0; |
maetugr | 2:93f703d2c4d7 | 71 | if (Acc_angle < 0) |
maetugr | 2:93f703d2c4d7 | 72 | ledset += 1; |
maetugr | 2:93f703d2c4d7 | 73 | else |
maetugr | 2:93f703d2c4d7 | 74 | ledset += 8; |
maetugr | 2:93f703d2c4d7 | 75 | if (angle < 0) |
maetugr | 2:93f703d2c4d7 | 76 | ledset += 2; |
maetugr | 2:93f703d2c4d7 | 77 | else |
maetugr | 2:93f703d2c4d7 | 78 | ledset += 4; |
maetugr | 4:e96b16ad986d | 79 | //wait(0.1); |
maetugr | 2:93f703d2c4d7 | 80 | LEDs = ledset; |
maetugr | 0:0c4fafa398b4 | 81 | |
maetugr | 2:93f703d2c4d7 | 82 | //LEDs.rollnext(); |
maetugr | 0:0c4fafa398b4 | 83 | } |
maetugr | 0:0c4fafa398b4 | 84 | } |