Revision 3:a97f1d874f4e, committed 2012-10-04

Comitter:

maetugr

Date:

Thu Oct 04 19:28:30 2012 +0000

Parent:

2:93f703d2c4d7

Child:

4:e96b16ad986d

Commit message:

mehrere tests auf einmal als zwischenspeicher

Changed in this revision

--- 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);
     }
 }