Diff: main.cpp

Revision:

1:855d13c5051c

Parent:

0:f4e9301d548b

--- a/main.cpp	Wed May 30 10:51:50 2012 +0000
+++ b/main.cpp	Wed Aug 15 01:47:47 2012 +0000
@@ -1,5 +1,4 @@
 #include "mbed.h"
-#include "HMC5883.h"
 #include "BMA180.h"
 #include "ITG3200.h"
 Serial pc(USBTX, USBRX);
@@ -9,82 +8,66 @@
 Timer GlobalTime;
 
 #define PI             3.1415926535897932384626433832795
-HMC5883 Mag(I2CBus, GlobalTime);
+#define Rad2Dree       57.295779513082320876798154814105
+
+float R;
 BMA180 Acc(I2CBus, GlobalTime);
 ITG3200 Gyro(I2CBus, GlobalTime);
 
-int HMC5883_getAngle(float x, float y)
-{
-float heading = atan2((float)y,(float)x); 
-// Your mrad result / 1000.00 (to turn it into radians).
-  float declinationAngle = 21.18  / 1000.0;
-  // If you have an EAST declination, use += declinationAngle, if you have a WEST declination, use -= declinationAngle
-  heading += declinationAngle;  
-if(heading < 0)  heading += 2*PI;// Correct for when signs are reversed.
-if(heading > 2*PI)   heading -= 2*PI; // Check for wrap due to addition of declination.
-return(heading * 180/PI); // Convert radians to degrees for readability.
-}
-
 
 int main() 
 {
-    pc.baud(115200);
+    pc.baud(9600);
   
     I2CBus.frequency(400000);
     GlobalTime.start();
 //***
     Acc.Init();
     wait_ms(500);
-      //0.5 Sekunden kalibrieren
-    //1 g sind ca -2870 auf der 3. Achse
-    short Raw1g[3]= {0, 0, 0}; // -2870
+    
+    //User Calibration
+    short Raw1g[3]= {0, 0, 0};
+    Acc.userCalibration(Raw1g);
+    
+    //0.5s Calibration
     Acc.Calibrate(500, Raw1g);
 //***   
     Gyro.Init();
     wait_ms(500);
     
-    //0.5 Sekunden kalibrieren
+    //0.5s Calibration
     Gyro.Calibrate(500);
 //***  
-    
-    Mag.Init();
-    #if 1
-        Mag.AutoCalibration= 1;                     //In Echtzeit kalibrieren
-    #else
-        short MagRawMin[3]= {-400, -400, -400};     //Gespeicherte Werte
-        short MagRawMax[3]= {400, 400, 400};
-        Mag.Calibrate(MagRawMin, MagRawMax);
-    #endif
+    //Print the value for the Full-scale range
+    pc.printf("Full-scale Range: %i \n\r", Gyro.getInfo());
     
     while(1)
     {
-        Mag.Update();
-        
-        l1= Mag.MeasurementError;
+        //Aquire new values for the Gyro and Acc
         Acc.Update();
         Gyro.Update();
         
-        pc.printf("Gyro:%.3f %.3f %.3f   ",
-            (Gyro.Rate[0]),
-            (Gyro.Rate[1]),
-            (Gyro.Rate[2]));     
-        
-        
-        pc.printf("BMA180:%.3f %.3f %.3f  ",
+        //Calcuate the resulting vector R from the 3 acc axes
+        R = sqrt(std::pow(Acc.Acc[0] , 2) + std::pow(Acc.Acc[1] , 2) + std::pow(Acc.Acc[2] , 2));
+       
+        // Print the angle in degrees of all 3 axes
+        pc.printf("Angles in degrees Acc: %.6f %.6f %.6f  \n\r", 
+            Rad2Dree * acos(Acc.Acc[0]/R),
+            Rad2Dree * acos(Acc.Acc[1]/R),
+            Rad2Dree * acos(Acc.Acc[2]/R));
+            
+       //Print the g-force vector on each axis in m/s^s
+       /*pc.printf("BMA180:%.6f %.6f %.6f  \n\r",
             (Acc.Acc[0]),
             (Acc.Acc[1]),
-            (Acc.Acc[2]));
+            (Acc.Acc[2]));*/
         
-     
+        //Print the angular velocity in radians/sec of each axis
+        /*pc.printf("Gyro:%.6f %.6f %.6f   \n\r",
+            (Gyro.Rate[0]),
+            (Gyro.Rate[1]),
+            (Gyro.Rate[2]));*/     
 
-       // pc.printf("Min: %i %i %i Max: %i %i %i Scale: %.3f %.3f %.3f Offset: %.3i %.3i %.3i Mag: %.3f %.3f %.3f\r\n",
-        /*    Mag.RawMin[0], Mag.RawMin[1], Mag.RawMin[2],
-            Mag.RawMax[0], Mag.RawMax[1], Mag.RawMax[2],
-            Mag.Scale[0] , Mag.Scale[1] , Mag.Scale[2],
-            Mag.Offset[0], Mag.Offset[1], Mag.Offset[2],
-            Mag.Mag[0]   , Mag.Mag[1]   , Mag.Mag[2]);*/
-         printf("grader = %i \n",HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1]));
-        
-        wait_ms(20);
+        wait_ms(100);
     }
 }