Advertisement
Guest User

Untitled

a guest
May 25th, 2019
86
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 0.65 KB | None | 0 0
  1. #include <Wire.h>
  2. #include <MechaQMC5883.h>
  3.  
  4. MechaQMC5883 compass;
  5.  
  6. void setup() {
  7. Wire.begin();
  8. Serial.begin(9600);
  9. compass.init();
  10. }
  11.  
  12. void loop() {
  13. int x, y, z, azimuth;
  14. compass.read(&x, &y, &z, &azimuth);
  15.  
  16. int dx = 536, dy = -478;
  17. x -= dx; y -= dy;
  18.  
  19. Serial.print(" a: ");
  20.  
  21. double a, b; a = x; b = y;
  22. double ans;
  23. if (a == 0) {
  24. ans = acos(0);
  25. if (b < 0)
  26. ans += acos(-1);
  27. }
  28. else {
  29. ans = atan(b / a);
  30. if (a < 0) {
  31. ans += acos(-1);
  32. }
  33. else {
  34. if (b < 0)
  35. ans += 2 * acos(-1);
  36. }
  37. }
  38. Serial.print(ans * 180 / PI);
  39. Serial.println();
  40. delay(100);
  41. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement