1  // SPDX-FileCopyrightText: 2019 Bill Earl for Adafruit Industries
 2  //
 3  // SPDX-License-Identifier: MIT
 4  
 5  // **********************************************
 6  // Zax-O-Meter Sketch 
 7  // for the Adafruit LSM303 Magnetometer Breakout
 8  //
 9  // Written by Bill Earl for Adafruit Industries
10  //
11  // **********************************************
12  
13  #include <Wire.h>
14  #include <Adafruit_Sensor.h>
15  #include <Adafruit_LIS2MDL.h>
16  
17  //Uncomment this line and comment out the above for the LSM303DLH
18  //#include <Adafruit_LSM303DLH_Mag.h>
19  #include <Servo.h>
20  
21  /* Assign a unique ID to this sensor at the same time */
22  Adafruit_LIS2MDL mag = Adafruit_LIS2MDL(12345);
23  //Uncomment this line and comment out the above for the LSM303DLH
24  //Adafruit_LSM303DLH_Mag_Unified mag = Adafruit_LSM303AGR_Mag_Unified(12345);
25  
26  // This is our continuous rotation servo
27  Servo servo;
28  
29  // Pi for calculations - not the raspberry type
30  const float Pi = 3.14159;
31  
32  // This is the value that gives you minimal rotation on
33  // a continuous rotation servo.  It is usually about 90.
34  // adjust this value to give minimal rotation for your servo
35  const float ServoNeutral = 90;  
36  
37  // This is the desired direction of travel
38  // expressed as a 0-360 degree compass heading
39  // 0.0 = North
40  // 90.0 = East
41  // 180.0 = South
42  // 270 = West
43  const float targetHeading = 0.0;
44  
45  
46  void setup(void) 
47  {
48    Serial.begin(115200);
49    Serial.println("Magnetometer Test"); Serial.println("");
50    
51    /* Initialise the sensor */
52    if(!mag.begin())
53    {
54      /* There was a problem detecting the LSM303 ... check your connections */
55      Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
56      while(1);
57    }
58    
59    servo.attach(9);  // Attach servo to pin 9
60  
61  }
62  
63  void loop(void) 
64  {
65    /* Get a new sensor event */ 
66    sensors_event_t event; 
67    mag.getEvent(&event);
68       
69    // Calculate the angle of the vector y,x
70    float heading = (atan2(event.magnetic.y,event.magnetic.x) * 180) / Pi;
71    // Normalize to 0-360
72    if (heading < 0)
73    {
74      heading = 360 + heading;
75    }
76    
77    // Calculate the error between tha measured heading and the target heading.
78    float error = heading - targetHeading;
79    if (error > 180)
80    {
81      error = error - 360;  // for angles > 180, correct in the opposite direction.
82    }
83    // A non-zero difference between the heading and the 
84    // targetHeading will bias the servoNeutral value and 
85    // cause the servo to rotate back toward the targetHeading.
86    // The divisor is to reduce the reaction speed and avoid oscillations
87    servo.write(ServoNeutral + error / 4 );
88  
89    delay(40);
90  }