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?
User | Revision | Line number | New 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 |