1  // SPDX-FileCopyrightText: 2018 Dave Astels for Adafruit Industries
  2  //
  3  // SPDX-License-Identifier: MIT
  4  
  5  /* Circuit Playground Express compass. */
  6  
  7  /* Adafruit invests time and resources providing this open source code. */
  8  /* Please support Adafruit and open source hardware by purchasing */
  9  /* products from Adafruit! */
 10  
 11  /* Written by Dave Astels for Adafruit Industries */
 12  /* Copyright (c) 2018 Adafruit Industries */
 13  /* Licensed under the MIT license. */
 14  
 15  /* All text above must be included in any redistribution. */
 16  
 17  #include <Wire.h>
 18  #include <Adafruit_NeoPixel.h>
 19  #include <Adafruit_Sensor.h>
 20  #include <Adafruit_LSM303_U.h>
 21  #include <math.h>
 22  
 23  /* Assign a unique ID to this sensor at the same time */
 24  Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(12345);
 25  
 26  // Replace these two lines with the results of calibration
 27  //---------------------------------------------------------------------------
 28  
 29  float raw_mins[2] = {1000.0, 1000.0};
 30  float raw_maxes[2] = {-1000.0, -1000.0};
 31  //---------------------------------------------------------------------------
 32  
 33  float mins[2];
 34  float maxes[2];
 35  float corrections[2] = {0.0, 0.0};
 36  
 37  
 38  // Support both classic and express
 39  #ifdef __AVR__
 40  #define NEOPIXEL_PIN 17
 41  #else
 42  #define NEOPIXEL_PIN 8
 43  #endif
 44  
 45  // When we setup the NeoPixel library, we tell it how many pixels, and which pin to use to send signals.
 46  // Note that for older NeoPixel strips you might need to change the third parameter--see the strandtest
 47  // example for more information on possible values.
 48  Adafruit_NeoPixel strip = Adafruit_NeoPixel(10, NEOPIXEL_PIN, NEO_GRB + NEO_KHZ800);
 49  
 50  // Map direction pie slices (of 30 deg each) to a neopixel, or two for the missing ones at USB & power.
 51  int led_patterns[12][2] = {{4, 5}, {5, -1}, {6, -1}, {7, -1}, {8, -1}, {9, -1}, {9, 0}, {0 -1}, {1, -1}, {2, -1}, {3, -1}, {4, -1}};
 52  
 53  #define BUTTON_A 4
 54  
 55  void fill(int red, int green, int blue) {
 56    for (int i = 0; i < 10; i++) {
 57      strip.setPixelColor(i, red, green, blue);
 58    }
 59    strip.show();
 60  }
 61  
 62  
 63  // Do some initial reading to let the magnetometer settle in.
 64  // This was found by experience to be required.
 65  // Indicated to the user by blue LEDs.
 66  void warm_up(void)
 67  {
 68    sensors_event_t event;
 69    fill(0, 0, 64);
 70    for (int ignore = 0; ignore < 100; ignore++) {
 71      mag.getEvent(&event);
 72      delay(10);
 73    }
 74  }
 75  
 76  
 77  // Find the range of X and Y values.
 78  // User needs to rotate the CPX a bunch during this.
 79  // Can be refined by doing more of the saem by pressing the A button.
 80  // Indicated to the user by green LEDs.
 81  void calibrate(bool do_the_readings)
 82  {
 83    sensors_event_t event;
 84    float values[2];
 85  
 86    fill(0, 64, 0);
 87  
 88    if (do_the_readings) {
 89      unsigned long start_time = millis();
 90      while (millis() - start_time < 5000) {
 91  
 92        mag.getEvent(&event);
 93        values[0] = event.magnetic.x;
 94        values[1] = event.magnetic.y * -1;
 95        if (values[0] != 0.0 && values[1] != 0.0) { /* ignore the random zero readings... it's bogus */
 96          for (int i = 0; i < 2; i++) {
 97            raw_mins[i] = values[i] < raw_mins[i] ? values[i] : raw_mins[i];
 98            raw_maxes[i] = values[i] > raw_maxes[i] ? values[i] : raw_maxes[i];
 99          }
100        }
101        delay(5);
102      }
103    }
104  
105    for (int i = 0; i < 2; i++) {
106      corrections[i] = (raw_maxes[i] + raw_mins[i]) / 2;
107      mins[i] = raw_mins[i] - corrections[i];
108      maxes[i] = raw_maxes[i] - corrections[i];
109    }
110    fill(0, 0, 0);
111  }
112  
113  
114  void setup(void)
115  {
116    strip.begin();
117    strip.show();
118  
119    pinMode(BUTTON_A, INPUT_PULLDOWN);
120  
121    /* Enable auto-gain */
122    mag.enableAutoRange(true);
123  
124    /* Initialise the sensor */
125    if(!mag.begin())
126    {
127      /* There was a problem detecting the LSM303 ... check your connections */
128      fill(255, 0, 0);
129      while(1);
130    }
131  
132    warm_up();
133  
134    // If reset with button A pressed or calibration hasn't been done, run calibration and report the results
135    if (digitalRead(BUTTON_A) || (raw_mins[0] == 1000.0 && raw_mins[1] == 1000.0)) {
136      while (!Serial);
137      Serial.begin(9600);
138      Serial.println("Compass calibration\n");
139  
140      raw_mins[0] = 1000.0;
141      raw_mins[1] = 1000.0;
142      raw_maxes[0] = -1000.0;
143      raw_maxes[1] = -1000.0;
144      calibrate(true);
145  
146      Serial.println("Calibration results\n");
147      Serial.println("Update the corresponding lines near the top of the code\n");
148      Serial.print("float raw_mins[2] = {"); Serial.print(raw_mins[0]); Serial.print(", "); Serial.print(raw_mins[1]); Serial.println("};");
149      Serial.print("float raw_maxes[2] = {"); Serial.print(raw_maxes[0]); Serial.print(", "); Serial.print(raw_maxes[1]); Serial.println("};\n");
150  
151      while(1);
152    } else {
153      calibrate(false);
154    }
155  }
156  
157  
158  // Map a value from the input range to the output range
159  // Used to map MAG values from the calibrated (min/max) range to (-100, 100)
160  float normalize(float value, float in_min, float in_max) {
161    float mapped = (value - in_min) * 200 / (in_max - in_min) + -100;
162    float max_clipped = mapped <  100 ? mapped : 100;
163    float min_clipped = max_clipped > -100 ? max_clipped : -100;
164    return min_clipped;
165  }
166  
167  
168  void loop(void)
169  {
170    // Pressing button A does another round of calibration.
171    if (digitalRead(BUTTON_A)) {
172      calibrate(true);
173    }
174  
175    sensors_event_t event;
176    mag.getEvent(&event);
177  
178    float x = event.magnetic.x;
179    float y = event.magnetic.y * -1;
180  
181    if (x == 0.0 && y == 0.0) {
182      return;
183    }
184  
185    float normalized_x = normalize(x - corrections[0], mins[0], maxes[0]);
186    float normalized_y = normalize(y - corrections[1], mins[1], maxes[1]);
187  
188    int compass_heading = (int)(atan2(normalized_y, normalized_x) * 180.0 / 3.14159);
189    // compass_heading is between -180 and +180 since atan2 returns -pi to +pi
190    // this translates it to be between 0 and 360
191    compass_heading += 180;
192  
193    // We add 15 to account to the zero position being 0 +/- 15 degrees.
194    // mod by 360 to keep it within a circle
195    // divide by 30 to find which pixel corresponding pie slice it's in
196    int direction_index = ((compass_heading + 15) % 360) / 30;
197  
198    // light the pixel(s) for the direction the compass is pointing
199    // the empty spots where the USB and power connects are use the two leds to either side.
200    int *leds;
201    leds = led_patterns[direction_index];
202    for (int pixel = 0; pixel < 10; pixel++) {
203      if (pixel == leds[0] || pixel == leds[1]) {
204        strip.setPixelColor(pixel, 4, 0, 0);
205      } else {
206        strip.setPixelColor(pixel, 0, 0, 0);
207      }
208    }
209    strip.show();
210    delay(50);
211  }