Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Wire.h>
- #include <MechaQMC5883.h>
- MechaQMC5883 compass;
- void setup() {
- Wire.begin();
- Serial.begin(9600);
- compass.init();
- }
- void loop() {
- int x, y, z, azimuth;
- compass.read(&x, &y, &z, &azimuth);
- int dx = 536, dy = -478;
- x -= dx; y -= dy;
- Serial.print(" a: ");
- double a, b; a = x; b = y;
- double ans;
- if (a == 0) {
- ans = acos(0);
- if (b < 0)
- ans += acos(-1);
- }
- else {
- ans = atan(b / a);
- if (a < 0) {
- ans += acos(-1);
- }
- else {
- if (b < 0)
- ans += 2 * acos(-1);
- }
- }
- Serial.print(ans * 180 / PI);
- Serial.println();
- delay(100);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement