Mystick

#include <string.h>
#include <ctype.h>
#include <Wire.h>
#include <HMC5883L.h>
#include "LPD8806.h"
#include "SPI.h"

/*******************************************
 * Debugging variables
 *******************************************/
int ledPin = 13;
// Preset Locations: { near Campanile, near Soda, near Berkeley BART, near College & Parker }
const float PRESET_LAT[] = { 37.8721, 37.8754, 37.8701, 37.8641 };
const float PRESET_LON[] = { -122.2584, -122.2587, -122.2679, -122.2543 };

// Debugging flags
const boolean USE_PRESET_LOCATIONS = true;
const int PRESET_STORED_LOCATION_INDEX = 1;
const int PRESET_CURRENT_LOCATION_INDEX = 0;
const int PRESET_START_ADVENTURE_LOCATION_INDEX = 2;
const boolean CONTROL_ADVENTURE_END_VIA_BUTTON = true;
boolean debug_showed_progress = false;

/*******************************************
 * GPS variables
 *******************************************/
const int rxPin = 0;
const char GPRstring[7] = "$GPRMC";
char gpsDataBuffer[300];
int gpsDataBufferIndex;
int gpsDataFieldIndices[13];

/*******************************************
 * Button variables
 *******************************************/
const int buttonPin = 12;
int buttonState;            // the current reading from the input pin
int lastButtonState = HIGH; // the previous reading from the input pin
long lastDebounceTime = 0;  // the last time the output pin was toggled

/*******************************************
 * Compass variables
 *******************************************/
HMC5883L compass;
const int compassSDApin = 2; // variable not used in code.
const int compassSCLpin = 3; // variable not used in code.
const int northPin = 4;
const int northEastPin = 5;
const int eastPin = 6;
const int southEastPin = 7;
const int southPin = 8;
const int southWestPin = 9;
const int westPin = 10;
const int northWestPin = 11;

/*******************************************
 * Record/playback variables
 *******************************************/
const int recordPlaybackPin = A1;
const int recordingPin = A4;
const long MAX_RECORD_LENGTH = 22000; // milliseconds
unsigned long recordBeginTime, recordLength;
const long INSTRUCTIONS_LENGTH = 4000;

/*******************************************
 * LED strip variables
 *******************************************/
// Number of RGB LEDs in strand:
int nLEDs = 46 - 18;

// Two output pins
int dataPin  = A2;
int clockPin = A3;

LPD8806 strip = LPD8806(nLEDs, dataPin, clockPin);


/*******************************************
 * State variables
 *******************************************/
const float DISTANCE_THRESHOLD = 20; // meters
boolean currentLocationValid = false;

float currentLat, currentLon;
float storedLat, storedLon;
float startAdventureLat, startAdventureLon;

unsigned long playbackBeginTime, showingProgressBeginTime;
boolean buttonPressed, buttonReleased;
boolean showingProgress = false;

enum State {
  ADVENTURE_BEGUN,
  PLAYBACK,
  NO_LOCATION_SELECTED,
  RECORDING,
  LOCATION_SELECTED
} state;

/*******************************************
 * setup()
 *******************************************/
void setup() {
  // GPS
  Serial.begin(4800);
  pinMode(rxPin, INPUT);
  Serial1.begin(4800);
  // Initialize buffer for received data
  gpsDataBufferIndex = 0;
  for (int i=0;i<300;i++){
    gpsDataBuffer[i]=' ';
  }

  // Button
  pinMode(buttonPin, INPUT_PULLUP);

  // Compass
  Wire.begin();
  compass = HMC5883L();
  int error = compass.SetScale(1.3); // Set the scale of the compass.
  if(error != 0) { // If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));
  }
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
  if(error != 0) { // If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));
  }
  pinMode(northPin, OUTPUT);
  pinMode(westPin, OUTPUT);
  pinMode(eastPin, OUTPUT);
  pinMode(southPin, OUTPUT);
  pinMode(northEastPin, OUTPUT);
  pinMode(southEastPin, OUTPUT);
  pinMode(southWestPin, OUTPUT);
  pinMode(northWestPin, OUTPUT);

  // Record/Playback Module
  pinMode(recordPlaybackPin, OUTPUT);
  digitalWrite(recordPlaybackPin, LOW);
  pinMode(recordingPin, OUTPUT);
  digitalWrite(recordingPin, HIGH);

  // LED strip
  strip.begin();
  strip.show();

  // Debugging
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, LOW);

  if (USE_PRESET_LOCATIONS) {
    currentLat = PRESET_LAT[PRESET_CURRENT_LOCATION_INDEX];
    currentLon = PRESET_LON[PRESET_CURRENT_LOCATION_INDEX];
    storedLat = PRESET_LAT[PRESET_STORED_LOCATION_INDEX];
    storedLon = PRESET_LON[PRESET_STORED_LOCATION_INDEX];
    startAdventureLat = PRESET_LAT[PRESET_START_ADVENTURE_LOCATION_INDEX];
    startAdventureLon = PRESET_LON[PRESET_START_ADVENTURE_LOCATION_INDEX];
    currentLocationValid = true;
    Serial.println("ADVENTURE_BEGUN");
    state = ADVENTURE_BEGUN;
    recordLength = 0;
  } else {
    Serial.println("NO_LOCATION_SELECTED");
    state = NO_LOCATION_SELECTED;
  }

  Serial.println("ready");
}

/*******************************************
 * loop()
 *******************************************/
void loop() {
  // Computes currentLat, currentLon if possible
  gpsModuleLoop();
  
  if (!currentLocationValid) {
    return;
  }

  // Check for button press
  buttonPressed = false;
  buttonReleased = false;
  buttonLoop();

  // Show compass direction
  // Compass direction only shows if state == ADVENTURE_BEGUN
  compassLoop();

  // State-based behavior
  // Some state-based behavior is in compassLoop()
  float progress =   (  distanceBetweenCoords(startAdventureLat, startAdventureLon, storedLat, storedLon) 
                      - distanceBetweenCoords(currentLat, currentLon, storedLat, storedLon) )
                   / distanceBetweenCoords(startAdventureLat, startAdventureLon, storedLat, storedLon);

  // Show progress for 3 seconds at a time
  if (showingProgress && millis() - showingProgressBeginTime > 3000) {
    lightsOff();
    showingProgress = false;
  }

  switch (state) {
    case ADVENTURE_BEGUN:
      // Check if reached stored location
      if (   distanceBetweenCoords(storedLat, storedLon, currentLat, currentLon) < DISTANCE_THRESHOLD
          || (CONTROL_ADVENTURE_END_VIA_BUTTON && buttonPressed && debug_showed_progress)) {
        // Reached stored location
        showingProgress = false;
        lightsOff();
        finalLights();
        lightsOff();
        Serial.println("PLAYBACK");
        state = PLAYBACK;
        playback();
        playbackBeginTime = millis();
      } else if (buttonPressed) {
        // Show Progress if button pressed in this state
        Serial.print("Show progress: ");
        Serial.println(progress);
        colorWipe(strip.Color( 127, 127, 127), 300, progress);
        showingProgress = true;
        showingProgressBeginTime = millis();
        debug_showed_progress = true;
      }
      break;

    case PLAYBACK:
      // Check if finished playback
      Serial.print(playbackBeginTime);
      Serial.print(" ");
      Serial.print(millis() - playbackBeginTime);
      Serial.print(" ");
      Serial.println(recordLength);
      
      if (millis() - playbackBeginTime > recordLength) {
        resetRecordPlaybackState();
        Serial.println("NO_LOCATION_SELECTED");
        state = NO_LOCATION_SELECTED;
      }
      break;

    case NO_LOCATION_SELECTED:
      if (buttonPressed) {
        // Store new location
        storedLat = currentLat;
        storedLon = currentLon;
        // Begin recording
        record();
        Serial.println("RECORDING");
        state = RECORDING;
      }
      break;

    case RECORDING:
      // Check if recording finished
      if (buttonReleased) {
        stopRecord();
        Serial.println("LOCATION_SELECTED");
        state = LOCATION_SELECTED;
      }
      break;

    case LOCATION_SELECTED:
      if (buttonPressed) {
        Serial.println("ADVENTURE_BEGUN");
        state = ADVENTURE_BEGUN;
        startAdventureLat = currentLat;
        startAdventureLon = currentLon;
        debug_showed_progress = false;
      }
      break;
  }  
}


/*******************************************
 * gpsModuleLoop()
 *
 * Based on example code for connecting a Parallax GPS module to the Arduino
 * Igor Gonzalez Martin. 05-04-2007
 * igor.gonzalez.martin@gmail.com
 * English translation by djmatic 19-05-2007
 * Listen for the $GPRMC string and extract the GPS location data from this.
 *******************************************/
void gpsModuleLoop() {
  int byteGPS=Serial1.read(); // Read a byte of the serial port
  if (byteGPS == -1) { // See if the port is empty yet
    digitalWrite(ledPin, HIGH);
    delay(100);
  } else {
    digitalWrite(ledPin, LOW);
//    Serial.write(byteGPS);
    // note: there is a potential buffer overflow here!
    gpsDataBuffer[gpsDataBufferIndex++]=byteGPS; // If there is serial port data, it is put in the buffer

    if (byteGPS==13){ // If the received byte is = to 13, end of transmission
      // note: the actual end of transmission is <CR><LF> (i.e. 0x13 0x10)
      int fieldsIndex=0;

      boolean correctString = true;
      // The following for loop starts at 1, because this code is clowny and the first byte is the <LF> (0x10) from the previous transmission.
      for (int i=1;i<7;i++){ // Verifies if the received command starts with $GPR
        if (gpsDataBuffer[i] != GPRstring[i-1]){
          correctString = false;
          break;
        }
      }

      if(correctString){ // If yes, fieldsIndexinue and process the data
        // Find field delimiters
        for (int i=0;i<300;i++){
          if (gpsDataBuffer[i]==',' || gpsDataBuffer[i]=='*') {
            // note: again, there is a potential buffer overflow here!
            gpsDataFieldIndices[fieldsIndex++]=i;
          }
        }
        
        if (gpsDataBuffer[gpsDataFieldIndices[1]+1] == 'A') {
          // If status is good, get current location
          getCurrentLatAndLon();
          currentLocationValid = true;
          Serial.print(currentLat, 4);
          Serial.print(", ");
          Serial.println(currentLon, 4);
        }
      }

      // Reset buffer
      gpsDataBufferIndex=0;
      for (int i=0;i<300;i++){
        gpsDataBuffer[i]=' ';             
      }
    }
  }
}

/*******************************************
 * getCurrentLatAndLon()
 *******************************************/
void getCurrentLatAndLon() {
  const int LAT_INDEX = 2;
  const int LAT_DIRECTION_INDEX = 3;
  const int LON_INDEX = 4;
  const int LON_DIRECTION_INDEX = 5;
  
  char currentLatStr[] = "DDMM.MMMM";
  char currentLonStr[] = "DDDMM.MMMM";
  
  copyGPSDataToBuffer(LAT_INDEX, currentLatStr);
  copyGPSDataToBuffer(LON_INDEX, currentLonStr);
  
  currentLat = convertGPSModuleFormatToDegrees(currentLatStr);
  currentLon = convertGPSModuleFormatToDegrees(currentLonStr);
  
  if (gpsDataBuffer[gpsDataFieldIndices[LAT_DIRECTION_INDEX]+1] == 'S') {
    currentLat = currentLat * -1;
  }
  if (gpsDataBuffer[gpsDataFieldIndices[LON_DIRECTION_INDEX]+1] == 'W') {
    currentLon = currentLon * -1;
  }

}

/*******************************************
 * copyGPSDataToBuffer()
 *******************************************/
void copyGPSDataToBuffer(int fieldIndex, char* buffer) {
  for (int j = gpsDataFieldIndices[fieldIndex]; j < (gpsDataFieldIndices[fieldIndex+1]-1); j++) {
    buffer[j - gpsDataFieldIndices[fieldIndex]] = gpsDataBuffer[j+1];
  }
}

/*******************************************
 * convertGPSModuleFormatToDegrees()
 *******************************************/
float convertGPSModuleFormatToDegrees(char* coordString)
{
  float floatVal = atof(coordString);
  int wholeDegrees = ((int) floatVal) / 100;
  float minutes = floatVal - (float) (wholeDegrees * 100);
  float finalDegrees = (float) (wholeDegrees + minutes / 60.0);
  return finalDegrees;
}

/*******************************************
 * distanceBetweenCoords()
 *
 * Returns value in meters.
 *******************************************/
float distanceBetweenCoords( float Lat1, float Lon1, float Lat2, float Lon2 )
{
  const float EARTH_RADIUS_METERS = 6372795.0;

  float dLat = radians( Lat2 - Lat1 );
  float dLon = radians( Lon2 - Lon1 );
  
  float a = sin( dLat / 2.0f ) * sin( dLat / 2.0f ) + 
            cos( radians( Lat1 ) ) * cos( radians( Lat2 ) ) *
            sin( dLon / 2.0f ) * sin( dLon / 2.0f );
  
  float d = 2.0f * atan2( sqrt( a ), sqrt( 1.0f - a ) );
  
  return d * EARTH_RADIUS_METERS;
}

/*******************************************
 * bearingBetweenCoords()
 *
 * returns value in degrees.
 *******************************************/
float bearingBetweenCoords(float Lat1, float Lon1, float Lat2, float Lon2) {
  float dLon = radians( Lon2 - Lon1 );

  Lat1 = radians(Lat1);
  Lat2 = radians(Lat2);
  Lon1 = radians(Lon1);
  Lon2 = radians(Lon2);
    
  float y = sin(dLon)*cos(Lat2);
  float x = cos(Lat1)*sin(Lat2) - sin(Lat1)*cos(Lat2)*cos(dLon);
  
  return (((int) degrees(atan2(y,x))) + 360) % 360;
}

/*******************************************
 * buttonLoop()
 *
 * based on Debounce example
 *******************************************/
void buttonLoop() {
  const long DEBOUNCE_DELAY = 100;    // the debounce time; increase if the output flickers

  // read the state of the switch into a local variable:
  int reading = digitalRead(buttonPin);

  // check to see if you just pressed the button 
  // (i.e. the input went from LOW to HIGH),  and you've waited 
  // long enough since the last press to ignore any noise:  

  // If the switch changed, due to noise or pressing:
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  } 
  
  if ((millis() - lastDebounceTime) > DEBOUNCE_DELAY) {
    // whatever the reading is at, it's been there for longer
    // than the debounce delay, so take it as the actual current state:

    // if the button state has changed:
    if (reading != buttonState) {
      buttonState = reading;

      if (buttonState == LOW) {
        buttonPressed = true;
      } else {
        buttonReleased = true;
      }
    }
  }
  
  // save the reading.  Next time through the loop,
  // it'll be the lastButtonState:
  lastButtonState = reading;
}

/*******************************************
 * compassLoop()
 *******************************************/
void compassLoop()
{
  // Retrive the raw values from the compass (not scaled).
  MagnetometerRaw raw = compass.ReadRawAxis();
  // Retrived the scaled values from the compass (scaled to the configured scale).
  MagnetometerScaled scaled = compass.ReadScaledAxis();
  
  // Values are accessed like so:
  int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)
  
  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(scaled.YAxis, scaled.XAxis);
  
  // Once you have your heading, you must then add your ‘Declination Angle’, which is the ‘Error’ of the magnetic field in your location.
  // Find yours here: http://www.magnetic-declination.com/
  float declinationAngle = -0.1742;
  heading += declinationAngle;
  
  // Correct for when signs are reversed.
  if(heading < 0)
  heading += 2*PI;
  
  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
  heading -= 2*PI;
  
  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI;
  
  Serial.println(headingDegrees); 

  // Invert headingDegrees so that instead of shining the LED
  // that corresponds to the direction, the LED shone is always
  // pointing north.
  headingDegrees = 360 - headingDegrees;

  if (state != ADVENTURE_BEGUN) {
    digitalWrite(northPin, LOW);
    digitalWrite(westPin, LOW);
    digitalWrite(eastPin, LOW);
    digitalWrite(southPin, LOW);
    digitalWrite(northEastPin, LOW);
    digitalWrite(southEastPin, LOW);
    digitalWrite(southWestPin, LOW);
    digitalWrite(northWestPin, LOW);
    return;
  } else {
    // Offset "0" to be in the direction of the stored location.
    headingDegrees = ((int) (headingDegrees + bearingBetweenCoords(currentLat, currentLon, storedLat, storedLon))) % 360;
  }
  //Light North LED
  if(headingDegrees > 337.5 or headingDegrees < 22.5)
  {
    digitalWrite(northPin, HIGH);
    digitalWrite(westPin, LOW);
    digitalWrite(eastPin, LOW);
    digitalWrite(southPin, LOW);
    digitalWrite(northEastPin, LOW);
    digitalWrite(southEastPin, LOW);
    digitalWrite(southWestPin, LOW);
    digitalWrite(northWestPin, LOW);
  }
  
  //Light South LED
  if(headingDegrees > 157.5 and headingDegrees < 202.5)
  {
    digitalWrite(northPin, LOW);
    digitalWrite(westPin, LOW);
    digitalWrite(eastPin, LOW);
    digitalWrite(southPin, HIGH);
    digitalWrite(northEastPin, LOW);
    digitalWrite(southEastPin, LOW);
    digitalWrite(southWestPin, LOW);
    digitalWrite(northWestPin, LOW);
  }
  
  //Light West LED
  if(headingDegrees > 247.5 and headingDegrees < 292.5)
  {
    digitalWrite(northPin, LOW);
    digitalWrite(westPin, HIGH);
    digitalWrite(eastPin, LOW);
    digitalWrite(southPin, LOW);
    digitalWrite(northEastPin, LOW);
    digitalWrite(southEastPin, LOW);
    digitalWrite(southWestPin, LOW);
    digitalWrite(northWestPin, LOW);
  }
  
  //Light East LED
  if (headingDegrees > 67.5 and headingDegrees < 112.5)
  {
    digitalWrite(northPin, LOW);
    digitalWrite(westPin, LOW);
    digitalWrite(eastPin, HIGH);
    digitalWrite(southPin, LOW);
    digitalWrite(northEastPin, LOW);
    digitalWrite(southEastPin, LOW);
    digitalWrite(southWestPin, LOW);
    digitalWrite(northWestPin, LOW);
  }
  
  //Light NorthEast LED
  if(headingDegrees > 22.5 and headingDegrees < 67.5)
  {
    digitalWrite(northPin, LOW);
    digitalWrite(westPin, LOW);
    digitalWrite(eastPin, LOW);
    digitalWrite(southPin, LOW);
    digitalWrite(northEastPin, HIGH);
    digitalWrite(southEastPin, LOW);
    digitalWrite(southWestPin, LOW);
    digitalWrite(northWestPin, LOW);
  }
  
    //Light SouthEast LED
  if(headingDegrees > 112.5 and headingDegrees < 157.5)
  {
    digitalWrite(northPin, LOW);
    digitalWrite(westPin, LOW);
    digitalWrite(eastPin, LOW);
    digitalWrite(southPin, LOW);
    digitalWrite(northEastPin, LOW);
    digitalWrite(southEastPin, HIGH);
    digitalWrite(southWestPin, LOW);
    digitalWrite(northWestPin, LOW);
  }
  
    //Light SouthWest LED
  if(headingDegrees > 202.5 and headingDegrees < 247.5)
  {
    digitalWrite(northPin, LOW);
    digitalWrite(westPin, LOW);
    digitalWrite(eastPin, LOW);
    digitalWrite(southPin, LOW);
    digitalWrite(northEastPin, LOW);
    digitalWrite(southEastPin, LOW);
    digitalWrite(southWestPin, HIGH);
    digitalWrite(northWestPin, LOW);
  }
  
    //Light NorthWest LED
  if(headingDegrees > 292.5 and headingDegrees < 337.5)
  {
    digitalWrite(northPin, LOW);
    digitalWrite(westPin, LOW);
    digitalWrite(eastPin, LOW);
    digitalWrite(southPin, LOW);
    digitalWrite(northEastPin, LOW);
    digitalWrite(southEastPin, LOW);
    digitalWrite(southWestPin, LOW);
    digitalWrite(northWestPin, HIGH);
  }
 
}

/*******************************************
 * record()
 *******************************************/
void record() {
  // To record, set record/playback pin low, then select message by setting message pin low
  digitalWrite(recordPlaybackPin, LOW);
  digitalWrite(recordingPin, LOW);
  recordBeginTime = millis();
  Serial.println(recordBeginTime);
}

/*******************************************
 * stopRecord()
 *******************************************/
void stopRecord() {
  resetRecordPlaybackState();
  unsigned long currentTime = millis();
  Serial.println(currentTime);
  Serial.println(recordBeginTime);
  Serial.println(currentTime - recordBeginTime);
  recordLength = currentTime - recordBeginTime;
  if (recordLength > MAX_RECORD_LENGTH) {
    recordLength = MAX_RECORD_LENGTH;
  }
  Serial.println(recordLength);
}

/*******************************************
 * playback()
 *******************************************/
void playback() {
  // To playback, set record/playback pin high, then select message by setting message pin low
  digitalWrite(recordPlaybackPin, HIGH);  
  digitalWrite(recordingPin, LOW);
}

/*******************************************
 * resetRecordPlaybackState()
 *******************************************/
void resetRecordPlaybackState() {
  digitalWrite(recordingPin, HIGH);
}

/*******************************************
 * colorWipe()
 *
 * Fill the dots progressively along the strip.
 *******************************************/
void colorWipe(uint32_t c, uint8_t wait, float progress) {
  float ledSoFar;
  
  ledSoFar = strip.numPixels()*(1-progress);
  for (int i=strip.numPixels(); i>=int(ledSoFar); i--) {
      strip.setPixelColor(i, c);
      strip.show();
      delay(wait);
 }
}

/*******************************************
 * finalLights()
 *
 * Show lights for reaching destination.
 *******************************************/
void finalLights() {

  flickerLights(strip.Color(  0,   0, 127), 200); // Blue
  flickerLights(strip.Color(  0,   127, 127), 200); // Cyan
  flickerLights(strip.Color(  127,   127, 127), 200); // White
  flickerLights(strip.Color(  0,   0, 127), 200); // Blue
  flickerLights(strip.Color(  0,   127, 127), 200); // Cyan
  flickerLights(strip.Color(  127,   127, 127), 200); // White
  
}

/*******************************************
 * flickerLights()
 *******************************************/
void flickerLights(uint32_t c, uint8_t wait) {
    for (int q=0; q < 3; q++) {
      for (int i=0; i < strip.numPixels(); i=i+3) {
        strip.setPixelColor(i+q, c);    //turn every third pixel on
      }
      strip.show();
     
      delay(wait);
     
      for (int i=0; i < strip.numPixels(); i=i+3) {
        strip.setPixelColor(i+q, 0);        //turn every third pixel off
      }
    }
}

/*******************************************
 * lightsOff()
 *******************************************/
void lightsOff() {
  for(int i=0; i<strip.numPixels(); i++) {
    strip.setPixelColor(i, 0);
  }
  strip.show();
  delay(200); // need delay after show?
}