Diff: main.cpp

Revision:

15:742683a8efda

Parent:

13:5f0a2103c707

Child:

16:59d80bf88bf8

--- a/main.cpp	Thu Aug 17 12:55:14 2017 +0000
+++ b/main.cpp	Wed Aug 23 11:53:22 2017 +0000
@@ -1,12 +1,29 @@
 #include <Timer.h>
 #include <math.h> 
+#include "mbed.h"
+#include "stdio.h"
 #include "deklaration.h"
 #include "messen.h"
-#include "mbed.h"
-#include "stdio.h"
+                          
+#define RAD 57.29577951
+
+uint16_t zeit;
+uint32_t zeit2;
+
+uint8_t k;
+
+
+                        
+             
                                 
 int main()
-{   
+{  
+    gain_g = 0;
+    z_off = 0;
+    zeit = 0;
+    k = 0;  
+    drift_z = 0;
+ 
     Motor1.period_ms(2);
     Motor2.period_ms(2);
     Motor3.period_ms(2);
@@ -17,12 +34,11 @@
     {   
         viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
     }  
-    
     if (taster3)            
     {         
         anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
     }
-    pc.printf("Druecke Taster1 für den Start\n\r");   
+    pc.printf("Druecke Taster1 fuer den Start\n\r");   
     n1 = n2 = n3 = n4 = 700;
     Motor1.pulsewidth_us(n1);
     Motor2.pulsewidth_us(n2);
@@ -30,10 +46,60 @@
     Motor4.pulsewidth_us(n4);  
     while(1)
     {
-        if (taster1) 
+        if (1)//(taster1) 
         {
-            while(!taster4) 
-            { 
+            while(1)//(!taster4) 
+            {                
+                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);
+                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);   
+                
+                /********//******/
+                /*Messen*//*Gyro*/
+                /********//******/   
+                timer.stop();
+                timer2.stop();                
+                timer.reset();
+                timer2.reset();
+                gain_g = 0;
+                pitch_g = 0;
+                roll_g = 0;
+                timer.start();
+                timer2.start();
+                int i = 0;
+                while(1)
+                {    
+                    i++;                   
+                    zeit = timer.read_us();
+                    timer.reset();
+                    aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
+                    gain_g = gain_g + ((z_g - z_off) * zeit * 0.000001 * 1/16.4);
+                    pitch_g = pitch_g + ((y_g  - y_off) * zeit * 0.000001 * 1/16.4);
+                    roll_g = roll_g + ((x_g - x_off) * zeit * 0.000001 * 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;
+                    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 = roll_g * 0.9 + roll_a * 0.1;
+                    if (i == 2000)
+                    {
+                        pc.printf("gain: %2.5f\tpitch: %2.5f\troll: %2.5f\t\n\r",pitch, roll, gain);
+                        i = 0;
+                    }
+                        
+                }
             }
         }
     }