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.
Diff: main.cpp
- Revision:
- 3:a97f1d874f4e
- Parent:
- 2:93f703d2c4d7
- Child:
- 4:e96b16ad986d
--- a/main.cpp Tue Oct 02 17:53:40 2012 +0000 +++ b/main.cpp Thu Oct 04 19:28:30 2012 +0000 @@ -10,7 +10,7 @@ TextLCD LCD(p5, p6, p7, p8, p9, p10, p11, TextLCD::LCD16x2); // RS, RW, E, D0, D1, D2, D3, Typ L3G4200D Gyro(p28, p27); ADXL345 Acc(p28, p27); -Servo Motor(p12); +//Servo Motor(p12); Timer GlobalTimer; @@ -22,38 +22,40 @@ LCD.cls(); // Display löschen LCD.printf("FlyBed v0.2"); LEDs.roll(2); - //LEDs = 15; + LEDs = 15; float Gyro_data[3]; int Acc_data[3]; - int Gyro_angle[3] = {0,0,0}; + //int Gyro_angle[3] = {0,0,0}; unsigned long dt = 0; unsigned long time_loop = 0; - Motor.initialize(); + //Motor.initialize(); float angle = 0;//TEMP - int j = 0; + float bangle = 0; + float drift = 0; //float drift = 0; GlobalTimer.start(); while(1) { - j++; + + Gyro.read(Gyro_data); Acc.read(Acc_data); - + // Acc data angle - float Acc_abs = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2)); + //float Acc_abs = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2)); //float Acc_angle = Rad2Deg * acos((float)Acc_data[2]/Acc_abs); - float Acc_angle = 0.9 * Rad2Deg * atan((float)Acc_data[1]/(float)Acc_data[2]); + float Acc_angle = Rad2Deg * atan2((float)Acc_data[1], (float)Acc_data[2]); //angle = (0.98)*(angle + Gyro_data[0] * 0.1) + (0.02)*(Acc_angle); - /*float messfrequenz = 10; - float geschwindigkeit = Gyro_data[0] - drift; - drift += (geschwindigkeit * messfrequenz * 0.3); - angle += (geschwindigkeit * messfrequenz); - angle += ((Acc_angle - angle) * messfrequenz * 0.1);*/ + float messfrequenz = 0.01; + float geschwindigkeit = Gyro_data[0] - drift; + //drift += (geschwindigkeit * messfrequenz * 0.3); + angle += (geschwindigkeit * messfrequenz); + angle += ((Acc_angle - angle) * messfrequenz * 0.1); //for (int i= 0; i < 3;i++) //drift[i] += (Gyro_data[i]-drift[i])* 0.1; @@ -65,16 +67,19 @@ dt = GlobalTimer.read_us() - time_loop; time_loop = GlobalTimer.read_us(); - angle += (Acc_angle - angle)/30 + Gyro_data[0] * 0.01; + bangle += (Acc_angle - angle)/50 + Gyro_data[0] * 0.001; //dt/10000000.0----------------------------------------- //Gyro_angle[0] += (Gyro_data[0]) * 0.01; - LCD.locate(0,0); - LCD.printf(" |%2.1f ",Acc_angle); //roll(x) pitch(y) yaw(z) + //LCD.printf("%2.1f %2.1f %2.1f", Gyro_data[0],Gyro_data[1],Gyro_data[2]); + //LCD.printf("%d %d |%2.1f ",Acc_data[1],Acc_data[2] ,Acc_angle); //roll(x) pitch(y) yaw(z) + LCD.printf(" |%2.1f ",Acc_data[2]/20.0); + LCD.locate(1,0); - //LCD.printf("%d %d %d %2.1f ", Acc_data[0],Acc_data[1],Acc_data[2], Acc_angle); - LCD.printf("%d %d %2.4f %2.1f ", Gyro_angle[0],Gyro_angle[1],dt/10000000.0, angle); + //LCD.printf("%d %d %d %2.1f ", Acc_data[0],Acc_data[1],Acc_data[2]); + //LCD.printf("%2.1f %2.1f %2.2f %2.1f", Acc_angle,Acc_angle,dt/10000.0, angle); + LCD.printf("%2.1f %2.1f %2.1f ",Acc_angle , bangle, angle); - Motor = 1000 + abs(Acc_data[1]); // Motorwert anpassen + //Motor = 1000 + abs(Acc_data[1]); // Motorwert anpassen //LED hin und her int ledset = 0; @@ -86,10 +91,9 @@ ledset += 2; else ledset += 4; - + wait(0.1); LEDs = ledset; //LEDs.rollnext(); - wait(0.1); } }