bool getPosition(void) ;
void serialFlush(void) ;
#include <SoftwareSerial.h>
SoftwareSerial GPS(10, 11);
float latitude, longitude, speed;
#define trigPin1 13
#define echoPin1 12
#define buzzerPin1 6
#define trigPin2 11
#define echoPin2 10
#define buzzerPin2 7
void setup() {
  Serial.begin(9600);
  GPS.begin(9600);
  pinMode(trigPin1, OUTPUT); 
  pinMode(echoPin1, INPUT);
  pinMode(buzzerPin1, OUTPUT); 
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(buzzerPin2, OUTPUT); 
  pinMode(2, OUTPUT);         // We will use the pin 2 as ground. We need to make sure it 
  digitalWrite(2,LOW);
  // is in LOW position so it works as a ground.
}
void loop() {
   calculateDistance(echoPin1, trigPin1,buzzerPin1);  //Get the distance for the left
  calculateDistance(echoPin2, trigPin2,buzzerPin2);  //Get the distance for the right
}
//**********************************************************************************
//***Function to measure time of return of an ultrasound echo.************
//***Set up for distances shorter than 2 m, which is enough for parking** 
//***Beyond that HC-SR04 is not reliable ***********************************
//**********************************************************************************
void calculateDistance(int echo, int trigger, int buzzer){
  long duration, distance;
 
  digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
duration = pulseIn(echoPin1, HIGH);   // Arduino’s built in function pulseIn reads a pulse
            // (either HIGH or LOW) on a pin. Returns the length of 
 // the pulse in microseconds or 0 if no complete pulse 
 // was received within the timeout. 
  distance = (duration) / 58.1;       // The speed of sound is 340 m/s or 29 microseconds 
     // The speed of sound is 340 m/s or 29 microseconds 
// per centimeter. 
// The ping travels out and back, so to find the distance 
// if the object we take half of the distance travelled.
  if (distance!=0){
Serial.println(distance); 
Serial.println(" cm");
delay(500);
if (distance<30){  //ตั้งระยะ
  long buzzer1;
tone(buzzer1, 1000); // ตั้งเสียง
  delay(100);        // ...for 1 sec
  noTone(buzzer1);     // Stop sound...
  delay(50);        // ...for 1sec
}
    else if (distance>50){
   long buzzer2;
    tone(buzzer2, 500); // Send 1KHz sound signal...
  delay(100);        // ...for 1 sec
  noTone(buzzer2);     // Stop sound...
  delay(100);        // ...for 1sec 
  }
}
}
    if (getPosition()) {
    Serial.print(latitude, 6);
    Serial.print(",");
    Serial.print(longitude, 6);
    Serial.print(" | Speed (Km) : ");
    Serial.print(speed, 2);
    Serial.println();
    delay(2000);
  }
bool getPosition() {
  if (GPS.available()) {
    String line = "";
    while(GPS.available()) {
      char c = GPS.read();
      if (c == '\r') {
        if (line.indexOf("$GPRMC") >= 0) {
          // Serial.println(line);
          String dataCut[13];
          int index = 0;
          for (int dataStart=0;dataStart<line.length();) {
            dataCut[index] = line.substring(dataStart+1, line.indexOf(',', dataStart+1));
            // Serial.println(dataCut[index]);
            dataStart = line.indexOf(',', dataStart+1);
            index++;
          }
          if (dataCut[2] == "A") {
            int dotPos = 0;
            dotPos = dataCut[3].indexOf('.');
            String latDeg = dataCut[3].substring(0, dotPos-2);
            String latMin = dataCut[3].substring(dotPos-2, dotPos+10);
            dotPos = dataCut[5].indexOf('.');
            String lngDeg = dataCut[5].substring(0, dotPos-2);
            String lngMin = dataCut[5].substring(dotPos-2, dotPos+10);
            latitude = (latDeg.toFloat() + (latMin.toFloat() / 60.0)) * (dataCut[4] == "N" ? 1 : -1);
            longitude = (lngDeg.toFloat() + (lngMin.toFloat() / 60.0)) * (dataCut[6] == "E" ? 1 : -1);
            speed = dataCut[7].toFloat() * 1.652;
            
            return true;
          } else {
            Serial.println("Error : No fix now.");
          }
          serialFlush();
        }
        line = "";
      } else if (c == '\n') {
        // pass
      } else {
        line += c;
      }
      delay(1);
    }
  }
  return false;
}
void serialFlush() {
  while(Serial.available()) Serial.read();
}
}