Diff: main.cpp

Revision:

24:aaa5b4703555

Parent:

23:c99a4bd60609

Child:

25:a8a3cbc57c61

--- a/main.cpp	Thu Sep 14 15:51:15 2017 +0000
+++ b/main.cpp	Thu Sep 14 16:41:02 2017 +0000
@@ -61,7 +61,7 @@
             flanke4 = taster4;
             if ((flanke4 != 0) && (hilfe4 == 0)) 
             {  
-                n1=n2=n3=n4=625;
+                n1=n2=n3=n4=650;
             }
             hilfe4=flanke4;
             Motor1.pulsewidth_us(n1);
@@ -94,10 +94,11 @@
         if (taster1) 
         {
             while(1)//(!taster4) 
-            {                
+            {   
+                uint16_t flanke1,hilfe1=0,flanke2,hilfe2=0,flanke3,hilfe3=0,flanke4,hilfe4=0;             
                 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
                 offset_gyro(&z_off, &x_off, &y_off);             
-                drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off);
+                //drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off);
                 pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off);  
                 pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);   
                 
@@ -117,46 +118,66 @@
                 while(1)
                 {    
                     i++;                   
-                    dt = timer.read_us()*0.000001;
+                    dt = timer.read_us() * 0.000001;
                     timer.reset();
                     aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
                     gain_g = ((z_g - z_off) * 1/16.4);
                     pitch_g = ((y_g  - y_off) * 1/16.4);
-                    roll_g = ((x_g - x_off) * 1/16.4);
+                    roll_g = ((x_g - x_off)  * 1/16.4);
                     y = y_a / 16384.00;      
                     x = x_a / 16384.00;      
                     z = z_a / 16384.00;      
                     roll_a = atan2(y, sqrt(x * x + z * z)) * RAD;
                     pitch_a = atan2(-x, z) * RAD;
-                    newAngle = roll_a;
-                    newRate = roll_g;
-                    if (timer2.read_ms() >= 2000)
+                    newAngle = pitch_a;
+                    newRate = pitch_g;
+                    /*if (timer2.read_ms() >= 2000)
                     {
                         gain_g -= drift_z;
                         pitch_g -= drift_y;
                         roll_g -= drift_x;
                         timer2.reset();
-                    }  
+                    }*/  
                     //gain = gain_g;
                     //pitch = pitch_g * 0.9 + pitch_a * 0.1;
-                    roll = getAngle(&newAngle, &newRate, &dt, &Q_angle, &Q_bias, &R_measure, &bias); 
+                    
+                    pitch = getAngle(&newAngle, &newRate, &dt, &Q_angle, &Q_bias, &R_measure, &bias); 
                     if (i == 2000)
                     {
-                        pc.printf("gain: %2.5f\tpitch: %2.5f\troll: %2.5f\t\n\r",roll);
+                        pc.printf("roll: %2.5f°\tnewAngle: %2.5f°\tnewRate: %2.5f°/s\tdt: %2.5fus\tDrehzahl: %d\n\r",pitch, newAngle, newRate, dt*1000000,n1);
                         i = 0;
                     }
-                    n1=n2=n3=n4=800;
+                    flanke2 = taster2;
+                    if ((flanke2 != 0) && (hilfe2 == 0)) 
+                    {  
+                        n1+=50;
+                        n2+=50;
+                        n3+=50;
+                        n4+=50;
+                    }
+                    hilfe2=flanke2;
+                    flanke3 = taster3;
+                    if ((flanke3 != 0) && (hilfe3 == 0)) 
+                    {  
+                        n1-=50;
+                        n2-=50;
+                        n3-=50;
+                        n4-=50;
+                    }
+                    hilfe3=flanke3;
+                    flanke4 = taster4;
+                    if ((flanke4 != 0) && (hilfe4 == 0)) 
+                    {  
+                        n1=n2=n3=n4=650;
+                    }
+                    hilfe4=flanke4;
                     Motor1.pulsewidth_us(n1);
                     Motor2.pulsewidth_us(n2);
                     Motor3.pulsewidth_us(n3);
-                    Motor4.pulsewidth_us(n4); 
-                    if(taster4){
-                            n1=n2=n3=n4=700;
-                    Motor1.pulsewidth_us(n1);
-                    Motor2.pulsewidth_us(n2);
-                    Motor3.pulsewidth_us(n3);
-                    Motor4.pulsewidth_us(n4); while(1);}
+                    Motor4.pulsewidth_us(n4);
                             
+                     
+                       
                 }
             }
         }