Diff: main.cpp

Revision:

18:78e7de067ddb

Parent:

17:de729cb71f1e

Child:

19:ac98ad146067

--- a/main.cpp	Mon Sep 11 07:49:00 2017 +0000
+++ b/main.cpp	Mon Sep 11 09:24:18 2017 +0000
@@ -4,6 +4,7 @@
 #include "stdio.h"
 #include "deklaration.h"
 #include "messen.h"
+#include "filter/Kalman.h"
                           
 #define RAD 57.29577951
 
@@ -12,7 +13,7 @@
 
 uint8_t k;
 
-
+Kalman Kalman();
                         
              
                                 
@@ -30,6 +31,7 @@
     Motor4.period_ms(2);
     initialisierung_gyro();
     initialisierung_acc();
+    
     if (taster2)
     {   
         viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
@@ -75,14 +77,15 @@
             Motor1.pulsewidth_us(n1);
             Motor2.pulsewidth_us(n2);
             Motor3.pulsewidth_us(n3);
-            Motor4.pulsewidth_us(n4);  
+            Motor4.pulsewidth_us(n4); 
+             
             pc.printf("Drehzahl=  %d\r= %d",n1);
         }    
     }      
     
     while(1)
     {
-        if (1)//(taster1) 
+        if (taster1) 
         {
             while(1)//(!taster4) 
             {                
@@ -111,14 +114,15 @@
                     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);
+                    gain_g = gain_g + ((z_g - z_off) * 1/16.4);
+                    pitch_g = pitch_g + ((y_g  - y_off) * 1/16.4);
+                    roll_g = 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;
+                    Kalman.getAngle(pitch_a,pitch_g,zeit);
                     if (timer2.read_ms() >= 2000)
                     {
                         gain_g -= drift_z;