DroneV004.c

/* This is a simple test, designed to make sure the MEGA 2560 is talking to your GPS EM-506 unit.
For this program, we are assuming you're using a MEGA 2560 and the "naked GPS" breakout board with
a bi-directional level shifter to interface the 3.3V pins of the GPS with the 5V pins of the MEGA.
Wiring should be done as follows:
GPS Pin 1: GND
GPS Pin 2: VCC +5V (this is not a mistake. The GPS unit itself has a voltage regulator that can handle 5V,
                    but the serial pins (TX RX) must be at 3.3V)
GPS Pin 3: RX pin (goes to TX1 Digital Pin 18 on MEGA via level shifter)
GPS Pin 4: TX pin (goes to RX1 Digital Pin 19 on MEGA via level shifter)
GPS Pin 5: GND
GPS Pin 6: Not connected

SD Card Pin layout:
CD - Not Connected
DO - Pin 50 via level shifter
GND - GND
SCK - Pin 52 via level shifter
VCC - 3.3V MEGA
DI - Pin 51 via level shifter
CS - Pin 53 via level shifter
*/

#include <TinyGPS.h>
#include <SPI.h>
#include <SD.h>

#define CS 53    // MEGA CS Pin
// For IC pin Layout, refer to SN754410 datasheet
#define DF 7  // IC pin 14 to brown wire on drive wheels
#define DR 6  // IC pin 11 to blue wire on drive wheels
#define SR 5  // IC pin 3 to brown wire on steer wheels
#define SL 4  // IC pin 6 to blue wire on steer wheels
#define startlat 40.1240959
#define startlon -88.2933426
#define destlat 40.1243935
#define destlon -88.2933350

TinyGPS gps;
File file;
float flat = 0;
float flon = 0;
int lognum = 1;
char filename[10] = {'\0'};
int counter = 0;

void setup()
{
  sprintf(filename, "log%d.csv", lognum);
  //Serial.begin(9600);    // debugging purposes only
  Serial1.begin(4800); // Serial1 refers to the TX1 and RX1 pins on the MEGA (digital pins 18 and 19, respectively)
  pinMode(DF, OUTPUT);
  pinMode(DR, OUTPUT);
  pinMode(SR, OUTPUT);
  pinMode(SL, OUTPUT);
  //Serial.print("Initializing SD card...");    // debugging purposes only
  pinMode(CS, OUTPUT);
  
  while(!SD.begin(CS))
  {
    SD.begin(CS);
    //Serial.println("Card failed... Trying again...");    // debugging purposes only
    delay(1000);
  }
  Serial.println("Card initialized.");
  
  file=SD.open(filename, FILE_WRITE);
  while(!file)
  {
    //Serial.println("error opening log.txt");  // debugging purposes only
    //Serial.println("Trying again...");        // debugging purposes only
    delay(1000);
  }
  file.println(" ");
  file.print("---------------New Test------------------");
  file.close();
  while (flat == 0 or flon == 0)
  {
    getgps();
  }
}

void loop()
{
  float diflat = fabs(flat - destlat);
  float diflon = fabs(flon - destlon);
  file=SD.open(filename, FILE_WRITE);
  file.print("Difference in LAT = ");
  file.println(diflat, 7);
  file.print("Difference in LON = ");
  file.println(diflon, 7);
  file.close();
  while ((diflat >= 0.0000501) or (diflon >= 0.0000501))
  {
    getgps();
    forward();
    diflat = fabs(flat - destlat);
    diflon = fabs(flon - destlon);
    file=SD.open(filename, FILE_WRITE);
    file.print("Difference in LAT = ");
    file.println(diflat, 7);
    file.print("Difference in LON = ");
    file.println(diflon, 7);
    file.close();
  }
  botstop();
  reverse();
  delay(50);
  botstop();
  delay(3000);
  getgps();
}
void getgps()
{
  bool newData = false;
  unsigned long chars;
  unsigned short sentences, failed;

  // For one second we parse GPS data and report some key values
  for (unsigned long start = millis(); millis() - start < 1000;)
  {
    while (Serial1.available())
    {
      char c = Serial1.read();
      //Serial.write(c); // uncomment this line if you want to see the raw GPS data every time it gets it
      // uncomment this block if you want to see the raw GPS data every time it gets it------------------
      //file=SD.open("log.txt", FILE_WRITE);
      //file.write(c);
      //file.close();
      //-------------------------------------------------------------------------------------------------
      if (gps.encode(c)) // Did a new valid sentence come in?
      {
        newData = true;
      }
    }
  }
  if (newData)
  {
    sprintf(filename, "log%d.csv", lognum);
    int year;
    byte month, day, hour, minutes, second, hundredths;
    unsigned long fix_age;
    int sat, prec;
    unsigned long age;
    gps.f_get_position(&flat, &flon, &age);
    gps.crack_datetime(&year, &month, &day, &hour, &minutes, &second, &hundredths, &fix_age);
    sat = gps.satellites() == TinyGPS::GPS_INVALID_SATELLITES ? 0 : gps.satellites();
    prec = gps.hdop() == TinyGPS::GPS_INVALID_HDOP ? 0 : gps.hdop();
    /* debugging purposes only----------------------------------------------------------
    Serial.print("LAT: ");
    Serial.println(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flat, 7);    
    Serial.print("LON: ");
    Serial.println(flon == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flon, 7);
    Serial.print("SAT: ");
    Serial.println(sat);
    Serial.print("PREC: ");
    Serial.println(prec);
    ----------------------------------------------------------------------------------*/
    // datalogging////////////////////////////////////////////////////////
    file=SD.open(filename, FILE_WRITE);
    while (!file)
    {
      file = SD.open(filename, FILE_WRITE);
      //Serial.print("SD card failed... trying again...");  // debugging purposes only
      delay(1000);
    }
    Serial.println("Writing to file...");
    file.println(" ");
    file.print(year);
    file.print("/");
    file.print(month);
    file.print("/");
    file.print(day);
    file.print(" ");
    file.print(hour - 6);
    file.print(":");
    file.print(minutes);
    file.print(":");
    file.println(second);
    file.print("LAT: ");
    file.println(flat, 7);
    file.print("LON: ");
    file.println(flon, 7);
    file.print("SAT: ");
    file.println(sat, 7);
    file.print("PREC: ");
    file.print(prec);
    file.close();
    counter++;
    Serial.println("Done");
    // datalogging complete////////////////////////////////////////////////
  }
  gps.stats(&chars, &sentences, &failed);
  /* debugging purposes only-------
  Serial.print("CSUM ERR: ");
  Serial.println(failed);
  Serial.println(" ");
  --------------------------------*/
  if (chars == 0)
  {
    //Serial.println("** No characters received from GPS: check wiring **");  // debugging purposes only
    file = SD.open(filename, FILE_WRITE);
    file.println("** No characters received from GPS: check wiring **");
    file.close();
  }
  if (counter > 1000)
  {
    lognum++;
    counter = 0;
  }
}
void forward()
{
  file=SD.open(filename, FILE_WRITE);
  file.println("Driving forward...");
  file.close();
  //Serial.println("Driving forward...");    // debugging purposes only
  digitalWrite(DF, HIGH);
  digitalWrite(DR, LOW);
}
void reverse()
{
  file=SD.open(filename, FILE_WRITE);
  file.println("Driving backward...");
  file.close();
  //Serial.println("Driving backward...");  // debugging purposes only
  digitalWrite(DR, HIGH);
  digitalWrite(DF, LOW);
}
void right()
{
  file=SD.open(filename, FILE_WRITE);
  file.println("Turning right...");
  file.close();
  //Serial.println("Turning right...");    // debugging purposes only
  digitalWrite(SR, HIGH);
  digitalWrite(SL, LOW);
}
void left()
{
  file=SD.open(filename, FILE_WRITE);
  file.println("Turning left...");
  file.close();
  //Serial.println("Turning left...");    // debugging purposes only
  digitalWrite(SL, HIGH);
  digitalWrite(SR, LOW);
}
void botstop()
{
  file=SD.open(filename, FILE_WRITE);
  file.println("All stop, all stop...");
  file.close();
  //Serial.println("All stop, all stop...");  // debugging purposes only
  digitalWrite(DF, LOW);
  digitalWrite(DR, LOW);
  digitalWrite(SR, LOW);
  digitalWrite(SL, LOW);
}