Diff: messen/messen.cpp

Revision:

15:742683a8efda

Parent:

14:a75b20f9cc24

Child:

25:a8a3cbc57c61

--- a/messen/messen.cpp	Thu Aug 17 12:55:14 2017 +0000
+++ b/messen/messen.cpp	Wed Aug 23 11:53:22 2017 +0000
@@ -1,9 +1,9 @@
 #include "mbed.h"
 #include "stdio.h"
-#define PI 3.14159
 
 
-int16_t high, low;
+
+
 
 extern Serial pc(SERIAL_TX, SERIAL_RX);
 extern SPI spi(PE_6,PE_5,PE_2);         //mosi,miso,sclk
@@ -21,10 +21,8 @@
                                     //  Gelb und Orange Vcc +5V
                                     //  Gnd Rot
                                     
-                                    
-
-                                    
-static int n1, n2, n3, n4;
+int n1, n2, n3, n4;
+int16_t high, low;
 
 /**********************/
 /*Initialisieren**Gyro*/
@@ -98,6 +96,86 @@
 }
 
 
+/*******************/
+/*Messen Sensor roh*/
+/*******************/
+
+void aktuell_roh(int16_t *z_g, int16_t *x_g, int16_t *y_g, int16_t *z_a, int16_t *x_a, int16_t *y_a)
+{  
+    ncs = 0;
+    spi.write(0xc7);                    
+    high = spi.write(0x0);
+    ncs = 1;
+    wait_us(0.1); 
+    ncs = 0;
+    spi.write(0xc8);                    
+    low = spi.write(0x0);
+    ncs = 1;
+    wait_us(0.1);         
+    (*z_g) = low | (high << 8);    
+    
+    ncs = 0;
+    spi.write(0xc3);                    
+    high = spi.write(0x0);
+    ncs = 1;
+    wait_us(1); 
+    ncs = 0;
+    spi.write(0xc4);                    
+    low = spi.write(0x0);
+    ncs = 1;
+    wait_us(0.1);           
+    (*x_g) = low | (high << 8);
+
+    ncs = 0;
+    spi.write(0xc5);                    
+    high = spi.write(0x0);
+    ncs = 1;
+    wait_us(0.1); 
+    ncs = 0;
+    spi.write(0xc6);                    
+    low = spi.write(0x0);
+    ncs = 1;
+    wait_us(0.1);
+    (*y_g) = low | (high << 8);     
+             
+    ncs=0;
+    spi.write(0xbb);            
+    high = spi.write(0x0);
+    ncs=1;
+    wait_us(0.1);
+    ncs=0;
+    spi.write(0xbc);            
+    low = spi.write(0x0);
+    ncs=1;
+    wait_us(0.1);
+    (*x_a) = low | (high << 8); 
+
+    ncs=0;
+    spi.write(0xbd);            
+    high = spi.write(0x0);
+    ncs=1;
+    wait_us(0.1);
+    ncs=0;
+    spi.write(0xbe);            
+    low = spi.write(0x0);
+    ncs=1;
+    wait_us(0.1);
+    (*y_a) = low | (high << 8); 
+
+    ncs=0;
+    spi.write(0xbf);            
+    high = spi.write(0x0);
+    ncs=1;
+    wait_us(0.1);  
+    ncs=0;
+    spi.write(0xc0);            
+    low = spi.write(0x0);
+    ncs=1;  
+    wait_us(0.1);
+    (*z_a) = low | (high << 8); 
+}
+
+
 /***************/
 /*Messen Gyro Z*/
 /***************/
@@ -178,15 +256,6 @@
     return low | high<<8; 
 }    
 
-float read_x_acc ()
-{ 
-    float xd = aktuell_acc_x();
-    xd/=16384;
-    if(xd<-1.0){return -1.0;}
-    if(xd>1.0){return 1.0;}
-    else return xd;
-}
-
 int16_t aktuell_acc_y ()
 {   
     ncs=0;
@@ -203,15 +272,6 @@
     return low | high<<8; 
 } 
 
-float read_y_acc ()
-{ 
-    float yd = aktuell_acc_y();
-    xd/=16384;
-    if(yd<-1.0){return -1.0;}
-    if(yd>1.0){return 1.0;}
-    else return yd;
-}
-
 int16_t aktuell_acc_z ()
 {   
     ncs=0;
@@ -228,36 +288,10 @@
     return low | high<<8; 
 } 
 
-float read_z_acc ()
-{ 
-    float zd = aktuell_acc_z();
-    zd/=16384;
-    if(zd<-1.0){return -1.0;}
-    if(zd>1.0){return 1.0;}
-    else return zd;
-}
-
-double pitch ()      
-{
-    double x,z; 
-    x=read_x_acc(); z=read_z_acc();
-    double pitch = atan2(-x,z)*180/PI;
-    return pitch;
-}
-
-double roll ()
-{
-    double x,y,z; 
-    y=read_y_acc(); x=pow(read_x_acc(),2); z=pow(read_z_acc(),2);
-    double roll = atan2(y,sqrt(x+z))*180/PI;
-    return roll;
-}
-
 
 /**********/
 /*Anlernen*/
 /**********/
-
 void anlernen(PwmOut *Motor1, PwmOut *Motor2, PwmOut *Motor3, PwmOut *Motor4, DigitalIn *taster1, DigitalIn *taster2, DigitalIn *taster4)
 {
     int n1 = n2 = n3 = n4 = 1900;
@@ -322,4 +356,105 @@
         (*rauschen) = 0;
         wait_ms(100000);
     }
-}
\ No newline at end of file
+}              
+/****************/
+/*Offset****Gyro*/
+/****************/   
+void offset_gyro(float *z_off, float *x_off, float *y_off)
+{
+    float z_off1, z_off2;
+    float x_off1, x_off2;
+    float y_off1, y_off2;
+    float i;
+    z_off2 = 1;
+    x_off2 = 1;
+    y_off2 = 1;
+    z_off2 = 1;
+    do
+    { 
+        for(i = 1; i <= 5000; i++)
+        {   
+            if ((z_off2 > 0.05) || (z_off2 < -0.05))
+            {
+                z_off1 += aktuell_gyro_z();
+            }
+            if ((y_off2 > 0.05) || (y_off2 < -0.05))
+            {
+                y_off1 += aktuell_gyro_y();
+            }
+            if ((x_off2 > 0.05) || (x_off2 < -0.05))
+            {
+                x_off1 += aktuell_gyro_x();
+            }                                                         
+                wait_ms(1);
+        }
+        if ((z_off2 > 0.05) || (z_off2 < -0.05))
+        {
+            z_off1 /= i;
+        }
+        if ((y_off2 > 0.05) || (y_off2 < -0.05))
+        {
+            y_off1 /= i;
+        }
+        if ((x_off2 > 0.05) || (x_off2 < -0.05))
+        {
+            x_off1 /= i;
+        }
+        for(i = 1; i <= 5000; i++)
+        { 
+            if ((z_off2 > 0.05) || (z_off2 < -0.05))
+            {
+                z_off2 += aktuell_gyro_z() - z_off1;
+            }
+            if ((y_off2 > 0.05) || (y_off2 < -0.05))
+            {
+                y_off2 += aktuell_gyro_y() - y_off1;
+            }
+            if ((x_off2 > 0.05) || (x_off2 < -0.05))
+            {
+                x_off2 += aktuell_gyro_x() - x_off1;
+            }         
+            wait_ms(1);
+        }   
+        if ((z_off2 > 0.05) || (z_off2 < -0.05))
+        {
+            z_off2 /= i;
+        }
+        if ((y_off2 > 0.05) || (y_off2 < -0.05))
+        {
+            y_off2 /= i;
+        }
+        if ((x_off2 > 0.05) || (x_off2 < -0.05))
+        {
+            x_off2 /= i;
+        }
+    } while(((z_off2 > 0.05) || (z_off2 < -0.05)) || ((y_off2 > 0.05) || (y_off2 < -0.05)) || ((x_off2 > 0.05) || (x_off2 < -0.05)));
+    (*z_off) = z_off1 + z_off2;
+    (*y_off) = y_off1 + y_off2;
+    (*x_off) = x_off1 + x_off2;
+}
+ 
+/****************/
+/*Drift*****Gyro*/
+/****************/  
+void drift_gyro(float *drift_z, float *drift_x, float *drift_y, Timer *timer, Timer *timer2, double *gain_g, double *roll_g, double *pitch_g, float *z_off, float *x_off, float *y_off)
+{
+    timer->stop();
+    timer2->stop();
+    timer->reset();
+    timer2->reset();
+    timer->start();
+    timer2->start();     
+    while(timer2->read_ms() <= 60000)
+    {   
+        uint32_t zeit = timer->read_us(); 
+        timer->reset();             
+        (*gain_g) = (*gain_g) + ((aktuell_gyro_z() - (*z_off)) * zeit * 0.000001 * 1/16.4);
+        (*pitch_g) = (*pitch_g) + ((aktuell_gyro_y() - (*y_off)) * zeit * 0.000001 * 1/16.4);
+        (*roll_g) = (*roll_g) + ((aktuell_gyro_x() - (*x_off)) * zeit * 0.000001 * 1/16.4);
+        
+    }
+    (*drift_z) = (*gain_g)/30000;    //Drift alle 4ms
+    (*drift_y) = (*pitch_g)/30000;    //Drift alle 4ms
+    (*drift_x) = (*roll_g)/30000;    //Drift alle 4ms
+}