main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include <HMC5883L.h>
00004 #include "hcsr04.h"
00005 #define PI 3.14159265359
00006 #define PERIOD 0.0001
00007 
00008 //Boussole
00009 HMC5883L toto(p28,p27);
00010 
00011 int main()
00012 {
00013    
00014 
00015     toto.setDefault();
00016     int Mx,My,Mz;
00017     float x,y,z;
00018   
00019   
00020  
00021     while(1) {
00022         //Boussole
00023         Mx=toto.getMx();
00024         My=toto.getMy();
00025         Mz=toto.getMz();
00026         if(Mx>32500) Mx=Mx-65535;
00027         if(My>32500) My=My-65535;
00028         if(Mz>32500) Mz=Mz-65535;
00029         x=atan2((float)My,(float)Mx);
00030         y=atan2((float)Mz,(float)Mx);
00031         z=atan2((float)Mz,(float)My);
00032         if(x<0)x+=2*PI;
00033         if(x>2*PI)x-=2*PI;
00034         if(y<0)y+=2*PI;
00035         if(y>2*PI)y-=2*PI;
00036         if(z<0)z+=2*PI;
00037         if(z>2*PI)z-=2*PI;
00038         x=x*180/PI;
00039         y=y*180/PI;
00040         z=z*180/PI;
00041         float error=90-y;
00042         printf("angle:%d\t %d\t%d\n\r",My,Mx,Mz);
00043        }
00044        
00045        }
00046 
00047