Dupuis sedid / Mbed 2 deprecated Test_Boussole_moteur

test

Dependencies:   HCSR04 HMC5883L mbed

main.cpp@0:85467ac58aa4, 2017-06-09 (annotated)

Committer:
sedid
Date:
Fri Jun 09 05:23:04 2017 +0000
Revision:
0:85467ac58aa4
test boussole;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sedid 0:85467ac58aa4 1 #include "mbed.h"
sedid 0:85467ac58aa4 2 #include "math.h"
sedid 0:85467ac58aa4 3 #include <HMC5883L.h>
sedid 0:85467ac58aa4 4 #include "hcsr04.h"
sedid 0:85467ac58aa4 5 #define PI 3.14159265359
sedid 0:85467ac58aa4 6 #define PERIOD 0.0001
sedid 0:85467ac58aa4 7
sedid 0:85467ac58aa4 8 //Boussole
sedid 0:85467ac58aa4 9 HMC5883L toto(p28,p27);
sedid 0:85467ac58aa4 10
sedid 0:85467ac58aa4 11 int main()
sedid 0:85467ac58aa4 12 {
sedid 0:85467ac58aa4 13
sedid 0:85467ac58aa4 14
sedid 0:85467ac58aa4 15 toto.setDefault();
sedid 0:85467ac58aa4 16 int Mx,My,Mz;
sedid 0:85467ac58aa4 17 float x,y,z;
sedid 0:85467ac58aa4 18
sedid 0:85467ac58aa4 19
sedid 0:85467ac58aa4 20
sedid 0:85467ac58aa4 21 while(1) {
sedid 0:85467ac58aa4 22 //Boussole
sedid 0:85467ac58aa4 23 Mx=toto.getMx();
sedid 0:85467ac58aa4 24 My=toto.getMy();
sedid 0:85467ac58aa4 25 Mz=toto.getMz();
sedid 0:85467ac58aa4 26 if(Mx>32500) Mx=Mx-65535;
sedid 0:85467ac58aa4 27 if(My>32500) My=My-65535;
sedid 0:85467ac58aa4 28 if(Mz>32500) Mz=Mz-65535;
sedid 0:85467ac58aa4 29 x=atan2((float)My,(float)Mx);
sedid 0:85467ac58aa4 30 y=atan2((float)Mz,(float)Mx);
sedid 0:85467ac58aa4 31 z=atan2((float)Mz,(float)My);
sedid 0:85467ac58aa4 32 if(x<0)x+=2*PI;
sedid 0:85467ac58aa4 33 if(x>2*PI)x-=2*PI;
sedid 0:85467ac58aa4 34 if(y<0)y+=2*PI;
sedid 0:85467ac58aa4 35 if(y>2*PI)y-=2*PI;
sedid 0:85467ac58aa4 36 if(z<0)z+=2*PI;
sedid 0:85467ac58aa4 37 if(z>2*PI)z-=2*PI;
sedid 0:85467ac58aa4 38 x=x*180/PI;
sedid 0:85467ac58aa4 39 y=y*180/PI;
sedid 0:85467ac58aa4 40 z=z*180/PI;
sedid 0:85467ac58aa4 41 float error=90-y;
sedid 0:85467ac58aa4 42 printf("angle:%d\t %d\t%d\n\r",My,Mx,Mz);
sedid 0:85467ac58aa4 43 }
sedid 0:85467ac58aa4 44
sedid 0:85467ac58aa4 45 }
sedid 0:85467ac58aa4 46
sedid 0:85467ac58aa4 47