Diff: main.cpp

Revision:

0:6bc5f85ca6fa

Child:

1:aea254b39529

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 21 13:48:10 2013 +0000
@@ -0,0 +1,69 @@
+#include "mbed.h"
+#include "HMC5883L.h"
+
+#include <math.h>
+
+#define SDA      p9
+#define SCL      p10
+#define PI       3.14159265
+
+int main()
+{      
+    float x, y, z, heading;
+    /*
+    float m_x, m_y, m_z;
+    */
+    printf("Inicializing...\r\n");
+    HMC5883L hmc5883l(SDA, SCL);
+    //HMC5883L *hmc5883l = new HMC5883L(SDA, SCL);
+    printf("OK...\r\n");
+    /*
+    for (int i = 0 ; i < 100 ; i++)
+    {
+        x += hmc5883l.getMx()/100;
+        y += hmc5883l.getMy()/100;
+        z += hmc5883l.getMz()/100;
+        wait_ms(100);
+    }
+    
+
+    m_x = x/2;
+    m_y = y/2;
+    m_z = z/2;
+    */
+    wait(1);
+
+    while(1) 
+    { 
+        /*
+        x = m_x - hmc5883l.getMx()*0.92;
+        y = m_y - hmc5883l.getMy()*0.92;
+        z = m_z - hmc5883l.getMz()*0.92;
+        */
+        x = hmc5883l.getMx();
+        y = hmc5883l.getMy();
+        z = hmc5883l.getMz();
+        
+        heading = atan2(y, x);
+        if(heading < 0) 
+            heading += 2*PI;
+        if(heading > 2*PI) 
+            heading -= 2*PI;
+        
+        heading = heading * 180 / PI;
+        
+        // Correct for when signs are reversed.
+        //if(heading < 0) 
+        //    heading += 2*PI;
+        //if(heading > 2*PI) 
+        //    heading -= 2*PI;
+       
+        //while(heading < 0) heading += 360;
+        //while(heading > 360) heading -= 360;
+
+
+        printf("x: %f \t\ty: %f \t\t z: %f \t\t heading: %f \t\r\n", x, y, z, heading);
+        wait_ms(200);
+
+    }
+}