1  // SPDX-FileCopyrightText: 2019 Becky Stern for Adafruit Industries
  2  // SPDX-FileCopyrightText: 2019 Justin Cooper for Adafruit Industries
  3  //
  4  // SPDX-License-Identifier: MIT
  5  
  6  // Code for the Adafruit FLora Citi Bike Helmet Tutorial
  7  // https://learn.adafruit.com/citi-bike-helmet
  8  // Becky Stern and Justin Cooper, Adafruit.com
  9  //
 10  // This code shows how to listen to the GPS module in an interrupt
 11  // which allows the program to have more 'freedom' - just parse
 12  // when a new NMEA sentence is available! Then access data when
 13  // desired.
 14  //
 15  // Tested and works great with the Adafruit Flora GPS module
 16  //    ------> https://adafruit.com/products/1059
 17  // Pick one up today at the Adafruit electronics shop 
 18  // and help support open source hardware & software! -ada
 19  
 20  #include <Adafruit_GPS.h>
 21  #include <SoftwareSerial.h>
 22  #include <Wire.h>
 23  #include <LSM303.h>
 24  #include <Adafruit_NeoPixel.h>
 25  
 26  LSM303 compass;
 27  
 28  #define LAT_LON_SIZE 311
 29  
 30  const float lat_lon[LAT_LON_SIZE][2] PROGMEM = {
 31    {40.767272, -73.993928},
 32    {40.719115, -74.006666},
 33    {40.711174, -74.000165},
 34    {40.683826, -73.976323},
 35    {40.741776, -74.001497},
 36    {40.696089, -73.978034},
 37    {40.686767, -73.959281},
 38    {40.731724, -74.006744},
 39    {40.727102, -74.002970},
 40    {40.714255, -73.981308},
 41    {40.692395, -73.993379},
 42    {40.698398, -73.980689},
 43    {40.716250, -74.009105},
 44    {40.715421, -74.011219},
 45    {40.720873, -73.980857},
 46    {40.721815, -73.997203},
 47    {40.714739, -74.009106},
 48    {40.690892, -73.996123},
 49    {40.729170, -73.998102},
 50    {40.753230, -73.970325},
 51    {40.748900, -73.976048},
 52    {40.739713, -73.994564},
 53    {40.760646, -73.984426},
 54    {40.738176, -73.977386},
 55    {40.709056, -74.010433},
 56    {40.743349, -74.006817},
 57    {40.700378, -73.995480},
 58    {40.702771, -73.993836},
 59    {40.690284, -73.987071},
 60    {40.737815, -73.999946},
 61    {40.711463, -74.005524},
 62    {40.741951, -74.008030},
 63    {40.727434, -73.993790},
 64    {40.695976, -73.990148},
 65    {40.692462, -73.989639},
 66    {40.728418, -73.987139},
 67    {40.730473, -73.986723},
 68    {40.736196, -74.008592},
 69    {40.691965, -73.981301},
 70    {40.689810, -73.974931},
 71    {40.697883, -73.973503},
 72    {40.688663, -73.980518},
 73    {40.691960, -73.965368},
 74    {40.693270, -73.977038},
 75    {40.735353, -74.004830},
 76    {40.721853, -74.007717},
 77    {40.718709, -74.009000},
 78    {40.724560, -73.995652},
 79    {40.723179, -73.994800},
 80    {40.732263, -73.998522},
 81    {40.735439, -73.994539},
 82    {40.735324, -73.998004},
 83    {40.719392, -74.002472},
 84    {40.689407, -73.968854},
 85    {40.701221, -74.012342},
 86    {40.703651, -74.011677},
 87    {40.694748, -73.983624},
 88    {40.691782, -73.973729},
 89    {40.717290, -73.996375},
 90    {40.707064, -74.007318},
 91    {40.722293, -73.991475},
 92    {40.723683, -73.975748},
 93    {40.750977, -73.987654},
 94    {40.719105, -73.999733},
 95    {40.693082, -73.971789},
 96    {40.685281, -73.978058},
 97    {40.686918, -73.976682},
 98    {40.686500, -73.965633},
 99    {40.717487, -74.010455},
100    {40.697665, -73.984764},
101    {40.699869, -73.982719},
102    {40.733319, -73.995101},
103    {40.708272, -73.968341},
104    {40.734545, -73.990741},
105    {40.684568, -73.958810},
106    {40.760202, -73.964784},
107    {40.713452, -73.983985},
108    {40.730286, -73.990764},
109    {40.730493, -73.995721},
110    {40.714066, -73.992939},
111    {40.714130, -73.997046},
112    {40.686832, -73.979677},
113    {40.728984, -73.990518},
114    {40.722174, -73.983687},
115    {40.720828, -73.977931},
116    {40.723627, -73.999496},
117    {40.704633, -74.013617},
118    {40.760957, -73.967244},
119    {40.708235, -74.005300},
120    {40.714274, -73.989900},
121    {40.713079, -73.998511},
122    {40.714978, -74.013012},
123    {40.689269, -73.989128},
124    {40.717227, -73.988020},
125    {40.722632, -73.988873},
126    {40.696102, -73.967510},
127    {40.694246, -73.992159},
128    {40.703553, -74.006702},
129    {40.709559, -74.006536},
130    {40.724537, -73.981854},
131    {40.753201, -73.977987},
132    {40.713361, -74.009376},
133    {40.717439, -74.005834},
134    {40.699917, -73.989717},
135    {40.696192, -73.991218},
136    {40.692361, -73.986317},
137    {40.689888, -73.981013},
138    {40.736245, -73.984737},
139    {40.729538, -73.984267},
140    {40.715337, -74.016583},
141    {40.724055, -74.009659},
142    {40.720434, -74.010206},
143    {40.714504, -74.005627},
144    {40.711731, -73.991930},
145    {40.712199, -73.979481},
146    {40.742387, -73.997262},
147    {40.729039, -73.994046},
148    {40.730477, -73.999060},
149    {40.703799, -74.008386},
150    {40.725806, -73.974224},
151    {40.712690, -73.987763},
152    {40.717821, -73.976289},
153    {40.717399, -73.980165},
154    {40.697940, -73.969868},
155    {40.685144, -73.953809},
156    {40.736494, -73.997043},
157    {40.736528, -74.006180},
158    {40.728738, -74.007488},
159    {40.724909, -74.001547},
160    {40.718502, -73.983298},
161    {40.715595, -73.987029},
162    {40.705309, -74.006125},
163    {40.763406, -73.977224},
164    {40.685395, -73.974314},
165    {40.693631, -73.962235},
166    {40.716021, -73.999743},
167    {40.716226, -73.982612},
168    {40.732617, -73.991580},
169    {40.732915, -74.007113},
170    {40.755102, -73.974986},
171    {40.707179, -74.008873},
172    {40.716058, -73.991907},
173    {40.751726, -73.987535},
174    {40.708346, -74.017134},
175    {40.689004, -73.960238},
176    {40.682231, -73.961458},
177    {40.693261, -73.968896},
178    {40.758280, -73.970694},
179    {40.730385, -74.002149},
180    {40.732241, -74.000263},
181    {40.694528, -73.958089},
182    {40.693317, -73.953819},
183    {40.726794, -73.996950},
184    {40.708621, -74.007221},
185    {40.722437, -74.005664},
186    {40.734011, -74.002938},
187    {40.734926, -73.992005},
188    {40.736251, -74.000836},
189    {40.683178, -73.965964},
190    {40.714948, -74.002344},
191    {40.712732, -74.004607},
192    {40.710445, -73.965250},
193    {40.692215, -73.984284},
194    {40.697601, -73.993445},
195    {40.695078, -73.987247},
196    {40.722992, -73.979954},
197    {40.725213, -73.977687},
198    {40.688070, -73.984106},
199    {40.680342, -73.955768},
200    {40.684157, -73.969222},
201    {40.691651, -73.999978},
202    {40.688515, -73.964762},
203    {40.719260, -73.981780},
204    {40.720195, -73.989978},
205    {40.740343, -73.989551},
206    {40.725028, -73.990696},
207    {40.740582, -74.005508},
208    {40.739323, -74.008119},
209    {40.695128, -73.995950},
210    {40.700101, -73.991043},
211    {40.710762, -73.994003},
212    {40.720664, -73.985179},
213    {40.722280, -73.976687},
214    {40.715815, -73.994223},
215    {40.702818, -73.987657},
216    {40.704717, -74.009260},
217    {40.687534, -73.972651},
218    {40.712912, -74.010202},
219    {40.702240, -73.982578},
220    {40.695807, -73.973555},
221    {40.687644, -73.969689},
222    {40.695733, -73.971296},
223    {40.770513, -73.988038},
224    {40.765849, -73.986905},
225    {40.717548, -74.013220},
226    {40.702515, -74.014270},
227    {40.724677, -73.987834},
228    {40.701485, -73.986569},
229    {40.688646, -73.982634},
230    {40.726217, -73.983798},
231    {40.729553, -73.980572},
232    {40.743174, -74.003664},
233    {40.741739, -73.994155},
234    {40.682165, -73.953990},
235    {40.680983, -73.950047},
236    {40.727791, -73.985649},
237    {40.726280, -73.989780},
238    {40.752554, -73.972826},
239    {40.756019, -73.967446},
240    {40.746841, -73.994458},
241    {40.708530, -73.964089},
242    {40.742354, -73.989150},
243    {40.727407, -73.981420},
244    {40.744876, -73.995298},
245    {40.763707, -73.985161},
246    {40.756603, -73.997900},
247    {40.764618, -73.987894},
248    {40.762272, -73.987882},
249    {40.744751, -73.999153},
250    {40.754557, -73.965929},
251    {40.750019, -73.969053},
252    {40.759710, -73.974023},
253    {40.766953, -73.981693},
254    {40.748061, -74.007231},
255    {40.745227, -74.007979},
256    {40.712858, -73.965902},
257    {40.735876, -73.982050},
258    {40.746919, -74.004518},
259    {40.742065, -74.004431},
260    {40.759345, -73.967596},
261    {40.755135, -73.986580},
262    {40.743954, -73.991448},
263    {40.683124, -73.978951},
264    {40.765265, -73.981923},
265    {40.763440, -73.982681},
266    {40.743453, -74.000040},
267    {40.712868, -73.956981},
268    {40.745712, -73.981948},
269    {40.721100, -73.991925},
270    {40.745167, -73.986830},
271    {40.735242, -73.987585},
272    {40.743943, -73.979660},
273    {40.756405, -73.990026},
274    {40.760300, -73.998842},
275    {40.760192, -73.991255},
276    {40.766696, -73.990617},
277    {40.712604, -73.962644},
278    {40.739355, -73.999317},
279    {40.732232, -73.988899},
280    {40.755002, -73.980144},
281    {40.750380, -73.983389},
282    {40.746200, -73.988557},
283    {40.733142, -73.975738},
284    {40.756458, -73.993722},
285    {40.750663, -74.001768},
286    {40.751575, -73.994190},
287    {40.740963, -73.986022},
288    {40.750199, -73.990930},
289    {40.756800, -73.982911},
290    {40.747348, -73.997235},
291    {40.762698, -73.993012},
292    {40.737261, -73.992389},
293    {40.737049, -73.990092},
294    {40.748548, -73.988084},
295    {40.762288, -73.983361},
296    {40.744219, -73.971212},
297    {40.738274, -73.987519},
298    {40.732218, -73.981655},
299    {40.749012, -73.988483},
300    {40.739126, -73.979737},
301    {40.763413, -73.996674},
302    {40.745497, -74.001971},
303    {40.760659, -73.980420},
304    {40.729386, -73.977724},
305    {40.750072, -73.998392},
306    {40.768254, -73.988639},
307    {40.760875, -74.002776},
308    {40.760094, -73.994618},
309    {40.752068, -73.967843},
310    {40.751492, -73.977988},
311    {40.747803, -73.973441},
312    {40.751884, -73.977701},
313    {40.759922, -73.976485},
314    {40.750449, -73.994810},
315    {40.757147, -73.972078},
316    {40.754665, -73.991381},
317    {40.755273, -73.983169},
318    {40.755941, -74.002116},
319    {40.747659, -73.984907},
320    {40.743155, -73.974347},
321    {40.742909, -73.977060},
322    {40.757569, -73.990985},
323    {40.771522, -73.990541},
324    {40.718939, -73.992662},
325    {40.710451, -73.960876},
326    {40.752996, -73.987216},
327    {40.702550, -74.012723},
328    {40.741443, -73.975360},
329    {40.740258, -73.984092},
330    {40.757952, -73.977876},
331    {40.715348, -73.960241},
332    {40.741472, -73.983209},
333    {40.736502, -73.978094},
334    {40.744449, -73.983035},
335    {40.702550, -73.989402},
336    {40.698920, -73.973329},
337    {40.716887, -73.963198},
338    {40.734160, -73.980242},
339    {40.725500, -74.004451},
340    {40.705311, -73.971000},
341    {40.765909, -73.976341}
342  };
343  
344  // Connect the GPS Power pin to 5V
345  // Connect the GPS Ground pin to ground
346  // If using software serial (sketch example default):
347  //   Connect the GPS TX (transmit) pin to Digital 8
348  //   Connect the GPS RX (receive) pin to Digital 7
349  // If using hardware serial:
350  //   Connect the GPS TX (transmit) pin to Arduino RX1 (Digital 0)
351  //   Connect the GPS RX (receive) pin to matching TX1 (Digital 1)
352  
353  // If using software serial, keep these lines enabled
354  // (you can change the pin numbers to match your wiring):
355  //SoftwareSerial mySerial(8, 7);
356  //Adafruit_GPS GPS(&mySerial);
357  
358  // If using hardware serial, comment
359  // out the above two lines and enable these two lines instead:
360  Adafruit_GPS GPS(&Serial1);
361  HardwareSerial mySerial = Serial1;
362  
363  // Set GPSECHO to 'false' to turn off echoing the GPS data to the Serial console
364  // Set to 'true' if you want to debug and listen to the raw GPS sentences
365  #define GPSECHO false
366  
367  Adafruit_NeoPixel strip = Adafruit_NeoPixel(45, 6, NEO_GRB + NEO_KHZ800);
368  int FarRight = 9;
369  int CenterRight = 10;
370  int CenterLeft = 34;
371  int FarLeft = 35;
372  int HeadsUp[] = {35, 34, 10, 9};
373  int RightStrip[] = {8, 7, 6, 5, 4, 3, 2, 1, 0};
374  int RightCenterStrip[] = {11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21};
375  int LeftCenterStrip[] = {33, 32, 31, 30, 29, 28, 27, 26, 25, 24, 23};
376  int LeftStrip[] = {36, 37, 38, 39, 40, 41, 42, 43, 44};
377  int counter = 0;
378  
379  // this keeps track of whether we're using the interrupt
380  // off by default!
381  boolean usingInterrupt = false;
382  
383  void setup()  
384  {
385    // connect at 115200 so we can read the GPS fast enough and echo without dropping chars
386    // also spit it out
387    Serial.begin(115200);
388    delay(5000);
389    
390    Serial.println("initialize");
391    //test to find the closest location from the list, just plug in a lat lon from the above Array
392    //Serial.print("Closest Test Loc: ");
393    //Serial.println(find_closest_location(40.726378, -74.005437));
394  
395    Wire.begin();
396    // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
397    GPS.begin(9600);
398    
399    // uncomment this line to turn on RMC (recommended minimum) and GGA (fix data) including altitude
400    GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
401    // uncomment this line to turn on only the "minimum recommended" data
402    //GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
403    // For parsing data, we don't suggest using anything but either RMC only or RMC+GGA since
404    // the parser doesn't care about other sentences at this time
405    
406    // Set the update rate
407    GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);   // 1 Hz update rate
408    // For the parsing code to work nicely and have time to sort thru the data, and
409    // print it out we don't suggest using anything higher than 1 Hz
410    compass.init();
411    compass.enableDefault();
412    // Calibration values. Use the Calibrate example program to get the values for
413    // your compass.
414    compass.m_min.x = -581; compass.m_min.y = -731; compass.m_min.z = -1097;
415    compass.m_max.x = +615; compass.m_max.y = +470; compass.m_max.z = 505;
416    delay(1000);
417    // Ask for firmware version
418    Serial1.println(PMTK_Q_RELEASE);
419    
420    strip.begin();
421    strip.show(); // Initialize all pixels to 'off'
422  }
423  
424  
425  
426  uint32_t timer = millis();
427  void loop()                     // run over and over again
428  {
429    // read data from the GPS in the 'main loop'
430    char c = GPS.read();
431    // if you want to debug, this is a good time to do it!
432    if (GPSECHO)
433        if (c) Serial.print(c);
434    
435    // if a sentence is received, we can check the checksum, parse it...
436    if (GPS.newNMEAreceived()) {
437      // a tricky thing here is if we print the NMEA sentence, or data
438      // we end up not listening and catching other sentences! 
439      // so be very wary if using OUTPUT_ALLDATA and trytng to print out data
440      //Serial.println(GPS.lastNMEA());   // this also sets the newNMEAreceived() flag to false
441    
442      if (!GPS.parse(GPS.lastNMEA()))   // this also sets the newNMEAreceived() flag to false
443        return;  // we can fail to parse a sentence in which case we should just wait for another
444    }
445    
446    // if millis() or timer wraps around, we'll just reset it
447    if (timer > millis())  timer = millis();
448  
449    // every 300 milliseconds, update the heading/distance indicators
450    if (millis() - timer > 300) { 
451      timer = millis(); // reset the timer
452      
453      //Serial.print("\nTime: ");
454      //Serial.print(GPS.hour, DEC); Serial.print(':');
455      //Serial.print(GPS.minute, DEC); Serial.print(':');
456      //Serial.print(GPS.seconds, DEC); Serial.print('.');
457      //Serial.println(GPS.milliseconds);
458      //Serial.print("Date: ");
459      //Serial.print(GPS.day, DEC); Serial.print('/');
460      //Serial.print(GPS.month, DEC); Serial.print("/20");
461      //Serial.println(GPS.year, DEC);
462      //Serial.print("Fix: "); Serial.print((int)GPS.fix);
463      //Serial.print(" quality: "); Serial.println((int)GPS.fixquality); 
464      if (GPS.fix) {
465        Serial.print("GPS FIX");
466        //Serial.print("Location: ");
467        //Serial.print(GPS.latitude, 2); Serial.print(GPS.lat);
468        //Serial.print(", "); 
469        //Serial.print(GPS.longitude, 2); Serial.println(GPS.lon);
470        
471        float fLat = decimalDegrees(GPS.latitude, GPS.lat);
472        float fLon = decimalDegrees(GPS.longitude, GPS.lon);
473        
474        int closest_loc = find_closest_location(fLat, fLon);
475        float targetLat = pgm_read_float(&lat_lon[closest_loc][0]);
476        float targetLon = pgm_read_float(&lat_lon[closest_loc][1]);
477        
478        //Serial.print("Speed (knots): "); Serial.println(GPS.speed);
479        //Serial.print("Angle: "); Serial.println(GPS.angle);
480        //Serial.print("Altitude: "); Serial.println(GPS.altitude);
481        //Serial.print("Satellites: "); Serial.println((int)GPS.satellites);
482        compass.read();
483        int heading = compass.heading((LSM303::vector<int16_t>){0,-1,0});
484        Serial.print("Heading: ");
485        Serial.println(heading);
486        if ((calc_bearing(fLat, fLon, targetLat, targetLon) - heading) > 0) {
487          headingDirection(calc_bearing(fLat, fLon, targetLat, targetLon)-heading);
488         }
489        else {
490          headingDirection(calc_bearing(fLat, fLon, targetLat, targetLon)-heading+360);
491        }
492        
493        //headingDistance((double)calc_dist(fLat, fLon, targetLat, targetLon));
494        //Serial.print("Distance Remaining:"); Serial.println((double)calc_dist(fLat, fLon, targetLat, targetLon));
495      }
496    }
497  }
498  
499  int calc_bearing(float flat1, float flon1, float flat2, float flon2)
500  {
501    float calc;
502    float bear_calc;
503  
504    float x = 69.1 * (flat2 - flat1); 
505    float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);
506  
507    calc=atan2(y,x);
508  
509    bear_calc= degrees(calc);
510  
511    if(bear_calc<=1){
512      bear_calc=360+bear_calc; 
513    }
514    return bear_calc;
515  }
516  
517  void headingDirection(float heading) 
518  {
519    //Use this part of the code to determine which way you need to go.
520    //Remember: this is not the direction you are heading, it is the direction to the destination (north = forward).
521    if ((heading > 348.75)||(heading < 11.25)) {
522      Serial.println("  N");
523      //Serial.println("Forward");
524      GoForward(strip.Color(16, 247, 206), 200);
525    }
526    
527    if ((heading >= 11.25)&&(heading < 33.75)) {
528      Serial.println("NNE");
529      //Serial.println("Go Right");
530      TurnRight(strip.Color(16, 247, 206), 200);
531    }  
532    
533    if ((heading >= 33.75)&&(heading < 56.25)) {
534      Serial.println(" NE");
535      //Serial.println("Go Right");
536      TurnRight(strip.Color(16, 247, 206), 200);
537    }
538    
539    if ((heading >= 56.25)&&(heading < 78.75)) {
540      Serial.println("ENE");
541      //Serial.println("Go Right");
542      TurnRight(strip.Color(16, 247, 206), 200);
543    }
544    
545    if ((heading >= 78.75)&&(heading < 101.25)) {
546      Serial.println("  E");
547      //Serial.println("Go Right");
548      TurnRight(strip.Color(16, 247, 206), 200);
549    }
550    
551    if ((heading >= 101.25)&&(heading < 123.75)) {
552      Serial.println("ESE");
553      //Serial.println("Go Right");
554      TurnRight(strip.Color(16, 247, 206), 200);
555    }
556    
557    if ((heading >= 123.75)&&(heading < 146.25)) {
558      Serial.println(" SE");
559      //Serial.println("Go Right");
560      TurnRight(strip.Color(16, 247, 206), 200);
561    }
562    
563    if ((heading >= 146.25)&&(heading < 168.75)) {
564      Serial.println("SSE");
565      //Serial.println("Go Right");
566      TurnRight(strip.Color(16, 247, 206), 200);
567    }
568    
569    if ((heading >= 168.75)&&(heading < 191.25)) {
570      Serial.println("  S");
571      //Serial.println("Turn Around");
572      TurnAround(strip.Color(247, 16, 70), 200);
573    }
574    
575    if ((heading >= 191.25)&&(heading < 213.75)) {
576      Serial.println("SSW");
577      //Serial.println("Go Left");
578      TurnLeft(strip.Color(16, 247, 206), 200);
579    }
580    
581    if ((heading >= 213.75)&&(heading < 236.25)) {
582      Serial.println(" SW");
583      //Serial.println("Go Left");
584      TurnLeft(strip.Color(16, 247, 206), 200);
585    }
586    
587    if ((heading >= 236.25)&&(heading < 258.75)) {
588      Serial.println("WSW");
589      //Serial.println("Go Left");
590      TurnLeft(strip.Color(16, 247, 206), 200);
591    }
592    
593    if ((heading >= 258.75)&&(heading < 281.25)) {
594      Serial.println("  W");
595      //Serial.println("Go Left");
596      TurnLeft(strip.Color(16, 247, 206), 200);
597    }
598    
599    if ((heading >= 281.25)&&(heading < 303.75)) {
600      Serial.println("WNW");
601      //Serial.println("Go Left");
602      TurnLeft(strip.Color(16, 247, 206), 200);
603    }
604    
605    if ((heading >= 303.75)&&(heading < 326.25)) {
606      Serial.println(" NW");
607      //Serial.println("Go Left");
608      TurnLeft(strip.Color(16, 247, 206), 200);
609    }
610    
611    if ((heading >= 326.25)&&(heading < 348.75)) {
612      Serial.println("NWN");
613      //Serial.println("Go Left");
614      TurnLeft(strip.Color(16, 247, 206), 200);
615    }
616  }
617  
618  unsigned long calc_dist(float flat1, float flon1, float flat2, float flon2)
619  {
620    float dist_calc=0;
621    float dist_calc2=0;
622    float diflat=0;
623    float diflon=0;
624  
625    diflat=radians(flat2-flat1);
626    flat1=radians(flat1);
627    flat2=radians(flat2);
628    diflon=radians((flon2)-(flon1));
629  
630    dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
631    dist_calc2= cos(flat1);
632    dist_calc2*=cos(flat2);
633    dist_calc2*=sin(diflon/2.0);
634    dist_calc2*=sin(diflon/2.0);
635    dist_calc +=dist_calc2;
636  
637    dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));
638  
639    dist_calc*=6371000.0; //Converting to meters
640    return dist_calc;
641  }
642  
643  //returns the location in the lat_lon array of the closest lat lon to the current location
644  int find_closest_location(float current_lat, float current_lon)
645  {  
646    int closest = 0;
647    signed long minDistance = -1;
648    signed long tempDistance;
649    for (int i=0; i < LAT_LON_SIZE; i++) {
650      float target_lat = pgm_read_float(&lat_lon[i][0]);
651      float target_lon = pgm_read_float(&lat_lon[i][1]);
652  
653      tempDistance = calc_dist(current_lat, current_lon, target_lat, target_lon);
654      
655      /*
656      Serial.print("current_lat: ");
657      Serial.println(current_lat, 6);
658      Serial.print("current_lon: ");
659      Serial.println(current_lon, 6); 
660      Serial.print("target_lat: ");
661      Serial.println(target_lat, 6);
662      Serial.print("target_lon: ");
663      Serial.println(target_lon, 6);  
664      
665      Serial.print("tempDistance: ");
666      Serial.println(tempDistance);
667      Serial.print("Array Loc: ");
668      Serial.println(i); 
669      */
670      
671      if ((minDistance > tempDistance) || (minDistance == -1)) {
672        minDistance = tempDistance;
673        closest = i;
674  
675      }
676   
677    }
678    return closest;
679  }
680  
681  // Convert NMEA coordinate to decimal degrees
682  float decimalDegrees(float nmeaCoord, char dir) {
683    uint16_t wholeDegrees = 0.01*nmeaCoord;
684    int modifier = 1;
685  
686    if (dir == 'W' || dir == 'S') {
687      modifier = -1;
688    }
689    
690    return (wholeDegrees + (nmeaCoord - 100.0*wholeDegrees)/60.0) * modifier;
691  }
692  
693  void TurnLeft (uint32_t c, uint8_t wait) {
694    strip.setPixelColor(CenterRight, 0);
695    strip.setPixelColor(CenterLeft, 0);
696    strip.setPixelColor(FarLeft, c);
697        strip.show();
698        delay(wait);
699    strip.setPixelColor(FarLeft, 0);
700    strip.show();
701        delay(wait);
702  }
703  
704  void TurnRight (uint32_t c, uint8_t wait) {
705    strip.setPixelColor(CenterRight, 0);
706    strip.setPixelColor(CenterLeft, 0);
707    strip.setPixelColor(FarRight, c);
708        strip.show();
709        delay(wait);
710    strip.setPixelColor(FarRight, 0);
711    strip.show();
712        delay(wait);
713  }
714  
715  void TurnAround (uint32_t c, uint8_t wait) {
716    strip.setPixelColor(CenterRight, c);
717    strip.setPixelColor(CenterLeft, c);
718        strip.show();
719        delay(wait);
720    strip.setPixelColor(CenterRight, 0);
721    strip.setPixelColor(CenterLeft, 0);
722    strip.show();
723        delay(wait);
724        
725  }
726  
727  void GoForward (uint32_t c, uint8_t wait) {
728    strip.setPixelColor(CenterRight, c);
729    strip.setPixelColor(CenterLeft, c);
730        strip.show();
731        
732        delay(wait);
733  }
734  
735  // Slightly different, this makes the rainbow equally distributed throughout
736  void rainbowCycle(uint8_t wait) {
737    uint16_t i;
738  
739    //for(j=0; j<256*5; j++) { // 5 cycles of all colors on wheel
740      for(i=0; i< 10; i++) {
741        strip.setPixelColor(RightStrip[i], Wheel(((i * 256 / 10) + counter) & 255));
742        strip.setPixelColor(RightCenterStrip[i], Wheel(((i * 256 / 10) + counter) & 255));
743        strip.setPixelColor(LeftCenterStrip[i], Wheel(((i * 256 / 10) + counter) & 255));
744        strip.setPixelColor(LeftStrip[i], Wheel(((i * 256 / 10) + counter) & 255));
745      }
746      counter = counter+25;
747      if (counter >256*5) {counter = 0;}
748      strip.show();
749      delay(wait);
750    //}
751  }
752  
753  
754  // Input a value 0 to 255 to get a color value.
755  // The colours are a transition r - g - b - back to r.
756  uint32_t Wheel(byte WheelPos) {
757    if(WheelPos < 85) {
758     return strip.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
759    } else if(WheelPos < 170) {
760     WheelPos -= 85;
761     return strip.Color(255 - WheelPos * 3, 0, WheelPos * 3);
762    } else {
763     WheelPos -= 170;
764     return strip.Color(0, WheelPos * 3, 255 - WheelPos * 3);
765    }
766  }