Diff: main.cpp

Revision:

0:85467ac58aa4

diff -r 000000000000 -r 85467ac58aa4 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 09 05:23:04 2017 +0000
@@ -0,0 +1,47 @@
+#include "mbed.h"
+#include "math.h"
+#include <HMC5883L.h>
+#include "hcsr04.h"
+#define PI 3.14159265359
+#define PERIOD 0.0001
+
+//Boussole
+HMC5883L toto(p28,p27);
+
+int main()
+{
+   
+
+    toto.setDefault();
+    int Mx,My,Mz;
+    float x,y,z;
+  
+  
+ 
+    while(1) {
+        //Boussole
+        Mx=toto.getMx();
+        My=toto.getMy();
+        Mz=toto.getMz();
+        if(Mx>32500) Mx=Mx-65535;
+        if(My>32500) My=My-65535;
+        if(Mz>32500) Mz=Mz-65535;
+        x=atan2((float)My,(float)Mx);
+        y=atan2((float)Mz,(float)Mx);
+        z=atan2((float)Mz,(float)My);
+        if(x<0)x+=2*PI;
+        if(x>2*PI)x-=2*PI;
+        if(y<0)y+=2*PI;
+        if(y>2*PI)y-=2*PI;
+        if(z<0)z+=2*PI;
+        if(z>2*PI)z-=2*PI;
+        x=x*180/PI;
+        y=y*180/PI;
+        z=z*180/PI;
+        float error=90-y;
+        printf("angle:%d\t %d\t%d\n\r",My,Mx,Mz);
+       }
+       
+       }
+
+