Spot

/*
	MicroView Arduino Library
	Copyright (C) 2014 GeekAmmo

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <MicroView.h>
#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>
#include <Time.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_NeoPixel.h>

SoftwareSerial mySerial(3, 2); // RX, TX


Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
Adafruit_GPS GPS(&mySerial);

#define WHEEL_SIZE 23
#define GPSECHO false
#define SECOND 00

//--------------------------------------------------|
//                    WAYPOINT                      |
//--------------------------------------------------|
//Please enter the latitude and longitude of your   |
//desired destination:                              |
#define GEO_LAT                48.009551
#define GEO_LON               -88.771131
//--------------------------------------------------|
//Your compass module may not line up with ours.    |
//Once you run compass mode, compare to a separate  |
//compass (like one found on your smartphone).      |
//Point your TOP_LED north, then count clockwise    |
//how many LEDs away from TOP_LED the lit LED is    |
#define LED_OFFSET             0
//--------------------------------------------------|
float fLat = 0.0;
float fLon = 0.0;
int currentGoal = 0;
int goalCount = 0;
float Lat[6];
float Lon[6];
char names[6];
char letters[6] = {'A', 'B', 'C', 'D', 'E', 'F'};
int totalLocs = 6;

const uint8_t maxW = uView.getLCDWidth();
const uint8_t midW = maxW/2;
const uint8_t maxH = uView.getLCDHeight();
const uint8_t midH = maxH/2;
// Navigation location
float targetLat = Lat[0];
float targetLon = Lon[0];
// this keeps track of whether we're using the interrupt
// off by default!
boolean usingInterrupt = false;
// Trip distance
float tripDistance;
// Offset hours from gps time (UTC)
//const int offset = 1;   // Central European Time
//const int offset = -4;  // Eastern Daylight Time (USA)
//const int offset = -5;  // Central Daylight Time (USA)
//const int offset = -8;  // Pacific Standard Time (USA)
const int offset = -7;  // Pacific Daylight Time (USA)
int compassOffset = LED_OFFSET;
int lastMin = 16;
int lastHour = 16;
int startLED = 0;
int startLEDlast = 16;
int lastCombined = 0;
int start = 0;
int mode = 0;
int lastDir = 16;
int dirLED_r = 0;
int dirLED_g = 0;
int dirLED_b = 255;
int compassReading;

// Calibration offsets
float magxOffset = 2.55;
float magyOffset = 27.95;


// Pushbutton setup
int button1 = 6; // the number of the pushbutton pin
int button2 = 5;
int button3 = A1;
int button4 = A0;
int buttonState;               
int lastButtonState = HIGH;    
long buttonHoldTime = 0;         // the last time the output pin was toggled
long buttonHoldDelay = 250;      // how long to hold the button down

// the following variables are long's because the time, measured in miliseconds,
// will quickly become a bigger number than can be stored in an int.
long lastDebounceTime = 0;     // the last time the output pin was toggled
long debounceDelay = 50;       // the debounce time; increase if the output flickers
long menuDelay = 250;
long menuTime;



void setup() {
	// Set the time in the time library:
  // connect at 115200 so we can read the GPS fast enough and echo without dropping chars
  // also spit it out
  Serial.begin(115200);
  Serial.println("Adafruit GPS library basic test!");

  // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
  GPS.begin(9600);
  // uncomment this line to turn on RMC (recommended minimum) and GGA (fix data) including altitude
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
  // uncomment this line to turn on only the "minimum recommended" data
  //GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
  // For parsing data, we don't suggest using anything but either RMC only or RMC+GGA since
  // the parser doesn't care about other sentences at this time
  // Set the update rate
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // 1 Hz update rate
  // For the parsing code to work nicely and have time to sort thru the data, and
  // print it out we don't suggest using anything higher than 1 Hz

  // Request updates on antenna status, comment out to keep quiet
  GPS.sendCommand(PGCMD_ANTENNA);
  
   // Make input & enable pull-up resistors on switch pins for pushbutton
  pinMode(button1, INPUT);
  pinMode(button2, INPUT);
  pinMode(button3, INPUT);
  pinMode(button4, INPUT);

  uView.begin();    // set up the MicroView
  uView.clear(PAGE);// erase hardware memory inside the OLED
  uView.display();  // display the content in the buffer

  // Draw clock face (circle outline & text):
  drawFace();
}
uint32_t gpsTimer = millis();
uint32_t startupTimer = millis();
uint32_t compassTimer = millis();
void loop() {
 //    Serial.println(digitalRead());   
  Serial.println(compassReading);
  // read the state of the switch into a local variable:
  
  //Serial.println(buttonState);
  // read data from the GPS in the 'main loop'
  char c = GPS.read();
  // if you want to debug, this is a good time to do it!
  if (GPSECHO)
    if (c) Serial.print(c);
  // if a sentence is received, we can check the checksum, parse it...
  if (GPS.newNMEAreceived()) {
    // a tricky thing here is if we print the NMEA sentence, or data
    // we end up not listening and catching other sentences!
    // so be very wary if using OUTPUT_ALLDATA and trytng to print out data
    Serial.println(GPS.lastNMEA()); // this also sets the newNMEAreceived() flag to false
    if (!GPS.parse(GPS.lastNMEA())) // this also sets the newNMEAreceived() flag to false
      return; // we can fail to parse a sentence in which case we should just wait for another
  }

  // if millis() or timer wraps around, we'll just reset it
  if (gpsTimer > millis()) gpsTimer = millis();

  if (start == 0) {
    if (GPS.fix) {
      // set the Time to the latest GPS reading
      setTime(GPS.hour, GPS.minute, GPS.seconds, GPS.day, GPS.month, GPS.year);
      delay(50);
      adjustTime(offset * SECS_PER_HOUR);
      delay(500);
      tripDistance = (double)calc_dist(fLat, fLon, targetLat, targetLon);
      start = 1;
    }
  }    
  // approximately every 60 seconds or so, update time
  if ((millis() - gpsTimer > 60000) && (start == 1)) {
    gpsTimer = millis(); // reset the timer
    if (GPS.fix) {
      // set the Time to the latest GPS reading
      setTime(GPS.hour, GPS.minute, GPS.seconds, GPS.day, GPS.month, GPS.year);
      delay(50);
      adjustTime(offset * SECS_PER_HOUR);
      delay(500);
    }
  }

  if (GPS.fix) {
    fLat = decimalDegrees(GPS.latitude, GPS.lat);
    fLon = decimalDegrees(GPS.longitude, GPS.lon);
  }
  
  //placesMode();
  if(digitalRead(button4) == HIGH){
    setMode();
  } 
}

void drawTime(float inputDegrees)
{
  static boolean firstDraw = false;
  static float degresssec, secx, secy;
  // If mSec
  
  
    // First time draw requires extra line to set up XOR's:
    if (firstDraw) 
    {
      uView.line(midW, midH, 32 + secx, 24 + secy, WHITE, XOR);
    }
    degresssec = (((inputDegrees * 360) / 60) + 270) * (PI / 180);

    // Calculate x,y coordinates of second hand:
    secx = cos(degresssec) * (WHEEL_SIZE / 1.1);
    secy = sin(degresssec) * (WHEEL_SIZE / 1.1);

    // Draw hands with the line function:
    uView.line(midW, midH, midW+secx, midH+secy, WHITE, XOR);
    
    // Set firstDraw flag to true, so we don't do it again.
    firstDraw = true;
    
    // Actually draw the hands with the display() function.
    uView.display();
  
}

// Draw the clock face. That includes the circle outline and
// the 12, 3, 6, and 9 text.
void drawFace()
{
  uView.setFontType(0); // set font type 0 (Smallest)
  
  uint8_t fontW = uView.getFontWidth();
  uint8_t fontH = uView.getFontHeight();
  
  //uView.setCursor(27, 0); // points cursor to x=27 y=0
  uView.setCursor(midW-(fontW/2)-1, midH-WHEEL_SIZE+1);
  uView.print("|");  // Print the "12"
  uView.setCursor(midW-(fontW/2)-1, midH+WHEEL_SIZE-fontH-1);
  uView.print("|");  // Print the "6"
  uView.setCursor(midW-WHEEL_SIZE+1, midH-fontH/2);
  uView.print("-");  // Print the "9"
  uView.setCursor(midW+WHEEL_SIZE-fontW-2, midH-fontH/2);
  uView.print("-");  // Print the "3"
  uView.circle(midW-1, midH-1, WHEEL_SIZE);
  //Draw the clock
  uView.display();
}
boolean finish = false;
void setMode() { 
  while(finish == false){
    float currentLat = decimalDegrees(GPS.latitude, GPS.lat);
    float currentLon = decimalDegrees(GPS.longitude, GPS.lon);
    Lat[goalCount] = currentLat;
    Lon[goalCount] = currentLon;
    names[goalCount] = letters[goalCount];
    uView.clear(PAGE);
    uView.setCursor(midW-1-20, midH-1);
    uView.print("Set as ");
    uView.print(names[goalCount]);
    uView.display();
    delay(2000);
    if(digitalRead(button3)== HIGH){
      finish = true;
      return;
    }
    
    }
   delay(100);
   finish = true;
  
}

void placesMode(){
  boolean finished = false;
  while(finished == false){  
    uView.clear(PAGE);
    int rowCount = 0;
    if(digitalRead(button2) == HIGH){
      rowCount++;
      uView.clear(PAGE);
      for(totalLocs; totalLocs < 5; totalLocs++){
        uView.setCursor(10, totalLocs);
        uView.print(names[totalLocs]);
      }
      uView.setCursor(0,rowCount*10);
      uView.print(">");
      uView.display();
    }
    else if(digitalRead(button3) == HIGH){
      rowCount--;
      uView.clear(PAGE);
      for(totalLocs; totalLocs < 5; totalLocs++){
        uView.setCursor(10, totalLocs);
        uView.print(names[totalLocs]);
      }
      uView.setCursor(0,rowCount);
      uView.print(">");
      uView.display();
    }
    else if(digitalRead(button1) == HIGH){
      uView.clear(PAGE);
      for(totalLocs; totalLocs < 5; totalLocs++){
        uView.setCursor(10, totalLocs);
        uView.print(names[totalLocs]);
      }
      uView.setCursor(0,rowCount);
      uView.print(">");
      uView.display();
      targetLat = Lat[rowCount];
      targetLon = Lon[rowCount];
      navMode();
      finished = true;
    }
    else {
      uView.clear(PAGE);
      for(totalLocs; totalLocs < 5; totalLocs++){
        uView.setCursor(10, totalLocs);
        uView.print(names[totalLocs]);
        uView.display();
      }
    }
  }
}

void navMode() {
  boolean finishy = false;
  while(finishy == false){
    if (start == 1) {
  
      compassCheck();
  
      if ((calc_bearing(fLat, fLon, targetLat, targetLon) - compassReading) > 0) {
        drawTime(calc_bearing(fLat, fLon, targetLat, targetLon)-compassReading);
      } 
      else {
        drawTime(calc_bearing(fLat, fLon, targetLat, targetLon)-compassReading+360);
      }
  
    } 
    else {
      // if millis() or timer wraps around, we'll just reset it
      if (startupTimer > millis()) startupTimer = millis();
  
      // approximately every 10 seconds or so, update time
      if (millis() - startupTimer > 200) {
        startupTimer = millis(); // reset the timer
        
      }
    }
    uView.display();
    
  }
  
}

int calc_bearing(float flat1, float flon1, float flat2, float flon2)
{
  float calc;
  float bear_calc;

  float x = 69.1 * (flat2 - flat1); 
  float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);

  calc=atan2(y,x);

  bear_calc= degrees(calc);

  if(bear_calc<=1){
    bear_calc=360+bear_calc; 
  }
  return bear_calc;
}


unsigned long calc_dist(float flat1, float flon1, float flat2, float flon2)
{
  float dist_calc=0;
  float dist_calc2=0;
  float diflat=0;
  float diflon=0;

  diflat=radians(flat2-flat1);
  flat1=radians(flat1);
  flat2=radians(flat2);
  diflon=radians((flon2)-(flon1));

  dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
  dist_calc2= cos(flat1);
  dist_calc2*=cos(flat2);
  dist_calc2*=sin(diflon/2.0);
  dist_calc2*=sin(diflon/2.0);
  dist_calc +=dist_calc2;

  dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));

  dist_calc*=6371000.0; //Converting to meters
  return dist_calc;
}

// Convert NMEA coordinate to decimal degrees
float decimalDegrees(float nmeaCoord, char dir) {
  uint16_t wholeDegrees = 0.01*nmeaCoord;
  int modifier = 1;

  if (dir == 'W' || dir == 'S') {
    modifier = -1;
  }

  return (wholeDegrees + (nmeaCoord - 100.0*wholeDegrees)/60.0) * modifier;
}


void compassCheck() {
  // if millis() or timer wraps around, we'll just reset it
  if (compassTimer > millis()) compassTimer = millis();

  // approximately every 10 seconds or so, update time
  if (millis() - compassTimer > 50) {
    /* Get a new sensor event */
    sensors_event_t event; 
    mag.getEvent(&event);

    float Pi = 3.14159;

    compassTimer = millis(); // reset the timer

    // Calculate the angle of the vector y,x
    float heading = (atan2(event.magnetic.y + magyOffset,event.magnetic.x + magxOffset) * 180) / Pi;

    // Normalize to 0-360
    if (heading < 0)
    {
      heading = 360 + heading;
    }
    compassReading = heading; 
    Serial.println(compassReading);
  }  
}