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@2:93f703d2c4d7, 2012-10-02 (annotated)
- Committer:
- maetugr
- Date:
- Tue Oct 02 17:53:40 2012 +0000
- Revision:
- 2:93f703d2c4d7
- Parent:
- 1:5a64632b1eb9
- Child:
- 3:a97f1d874f4e
erstes experiment mit funktionierendem filter (nur eine Achse und noch nicht optimal)
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 | 1:5a64632b1eb9 | 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 | 1:5a64632b1eb9 | 25 | //LEDs = 15; |
maetugr | 0:0c4fafa398b4 | 26 | |
maetugr | 2:93f703d2c4d7 | 27 | float Gyro_data[3]; |
maetugr | 0:0c4fafa398b4 | 28 | int Acc_data[3]; |
maetugr | 2:93f703d2c4d7 | 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 | 2:93f703d2c4d7 | 33 | Motor.initialize(); |
maetugr | 2:93f703d2c4d7 | 34 | |
maetugr | 2:93f703d2c4d7 | 35 | float angle = 0;//TEMP |
maetugr | 2:93f703d2c4d7 | 36 | int j = 0; |
maetugr | 2:93f703d2c4d7 | 37 | //float drift = 0; |
maetugr | 2:93f703d2c4d7 | 38 | GlobalTimer.start(); |
maetugr | 0:0c4fafa398b4 | 39 | while(1) { |
maetugr | 2:93f703d2c4d7 | 40 | j++; |
maetugr | 0:0c4fafa398b4 | 41 | |
maetugr | 0:0c4fafa398b4 | 42 | Gyro.read(Gyro_data); |
maetugr | 2:93f703d2c4d7 | 43 | Acc.read(Acc_data); |
maetugr | 2:93f703d2c4d7 | 44 | |
maetugr | 2:93f703d2c4d7 | 45 | // Acc data angle |
maetugr | 2:93f703d2c4d7 | 46 | 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 | 47 | //float Acc_angle = Rad2Deg * acos((float)Acc_data[2]/Acc_abs); |
maetugr | 2:93f703d2c4d7 | 48 | float Acc_angle = 0.9 * Rad2Deg * atan((float)Acc_data[1]/(float)Acc_data[2]); |
maetugr | 2:93f703d2c4d7 | 49 | |
maetugr | 2:93f703d2c4d7 | 50 | //angle = (0.98)*(angle + Gyro_data[0] * 0.1) + (0.02)*(Acc_angle); |
maetugr | 2:93f703d2c4d7 | 51 | |
maetugr | 2:93f703d2c4d7 | 52 | /*float messfrequenz = 10; |
maetugr | 2:93f703d2c4d7 | 53 | float geschwindigkeit = Gyro_data[0] - drift; |
maetugr | 2:93f703d2c4d7 | 54 | drift += (geschwindigkeit * messfrequenz * 0.3); |
maetugr | 2:93f703d2c4d7 | 55 | angle += (geschwindigkeit * messfrequenz); |
maetugr | 2:93f703d2c4d7 | 56 | angle += ((Acc_angle - angle) * messfrequenz * 0.1);*/ |
maetugr | 2:93f703d2c4d7 | 57 | |
maetugr | 2:93f703d2c4d7 | 58 | //for (int i= 0; i < 3;i++) |
maetugr | 2:93f703d2c4d7 | 59 | //drift[i] += (Gyro_data[i]-drift[i])* 0.1; |
maetugr | 2:93f703d2c4d7 | 60 | |
maetugr | 2:93f703d2c4d7 | 61 | //for (int i= 0; i < 3;i++) |
maetugr | 2:93f703d2c4d7 | 62 | //Gyro_angle[i] += (Gyro_data[i]/*-drift[i]*/); |
maetugr | 2:93f703d2c4d7 | 63 | |
maetugr | 2:93f703d2c4d7 | 64 | //dt berechnen |
maetugr | 2:93f703d2c4d7 | 65 | dt = GlobalTimer.read_us() - time_loop; |
maetugr | 2:93f703d2c4d7 | 66 | time_loop = GlobalTimer.read_us(); |
maetugr | 2:93f703d2c4d7 | 67 | |
maetugr | 2:93f703d2c4d7 | 68 | angle += (Acc_angle - angle)/30 + Gyro_data[0] * 0.01; |
maetugr | 2:93f703d2c4d7 | 69 | //Gyro_angle[0] += (Gyro_data[0]) * 0.01; |
maetugr | 0:0c4fafa398b4 | 70 | |
maetugr | 0:0c4fafa398b4 | 71 | LCD.locate(0,0); |
maetugr | 2:93f703d2c4d7 | 72 | LCD.printf(" |%2.1f ",Acc_angle); //roll(x) pitch(y) yaw(z) |
maetugr | 0:0c4fafa398b4 | 73 | LCD.locate(1,0); |
maetugr | 2:93f703d2c4d7 | 74 | //LCD.printf("%d %d %d %2.1f ", Acc_data[0],Acc_data[1],Acc_data[2], Acc_angle); |
maetugr | 2:93f703d2c4d7 | 75 | LCD.printf("%d %d %2.4f %2.1f ", Gyro_angle[0],Gyro_angle[1],dt/10000000.0, angle); |
maetugr | 2:93f703d2c4d7 | 76 | |
maetugr | 2:93f703d2c4d7 | 77 | Motor = 1000 + abs(Acc_data[1]); // Motorwert anpassen |
maetugr | 0:0c4fafa398b4 | 78 | |
maetugr | 2:93f703d2c4d7 | 79 | //LED hin und her |
maetugr | 2:93f703d2c4d7 | 80 | int ledset = 0; |
maetugr | 2:93f703d2c4d7 | 81 | if (Acc_angle < 0) |
maetugr | 2:93f703d2c4d7 | 82 | ledset += 1; |
maetugr | 2:93f703d2c4d7 | 83 | else |
maetugr | 2:93f703d2c4d7 | 84 | ledset += 8; |
maetugr | 2:93f703d2c4d7 | 85 | if (angle < 0) |
maetugr | 2:93f703d2c4d7 | 86 | ledset += 2; |
maetugr | 2:93f703d2c4d7 | 87 | else |
maetugr | 2:93f703d2c4d7 | 88 | ledset += 4; |
maetugr | 2:93f703d2c4d7 | 89 | |
maetugr | 2:93f703d2c4d7 | 90 | LEDs = ledset; |
maetugr | 0:0c4fafa398b4 | 91 | |
maetugr | 2:93f703d2c4d7 | 92 | //LEDs.rollnext(); |
maetugr | 0:0c4fafa398b4 | 93 | wait(0.1); |
maetugr | 0:0c4fafa398b4 | 94 | } |
maetugr | 0:0c4fafa398b4 | 95 | } |