/* 
 * Pure ranging application of the ultrasound sensor.
 * Daniel Sjoberg, 2019-01-27
 */

#define echoPin 13                       // Sensor Echo Pin connected to arduino pin 13
#define trigPin 12                       // Sensor Trigger Pin connected to arduino pin 12
#define powerPin 7                       // Sensor power supply taken from arduino pin 7

float duration, distance;                // Duration is used to calculate distance

// Timing parameters given in us
int delayRadarTrig1 = 10;                // Delay after first trig LOW
int delayRadarTrig2 = 10;                // Delay after second trig HIGH
int delayRadarWait = 100;                // Delay after each radar measurement
unsigned long timeout = 2000;            // Time to wait for the echo signal in us


void setup() {
  Serial.begin(9600);                    // Initialize serial port for communication
  pinMode(trigPin, OUTPUT);              // Define sensor trigger pin as output
  pinMode(echoPin, INPUT);               // Define sensor echo pin as input
  pinMode(powerPin, OUTPUT);             // Define power pin as output
  digitalWrite(powerPin, HIGH);          // Set power pin HIGH (+5V)
}

void Measure() {
  unsigned long t0=0, t1=0;              // Define the timeout for waiting on the pulse echo 
  
  digitalWrite(trigPin, LOW);            // Initialize trigger 
  delayMicroseconds(delayRadarTrig1);    // Wait
  digitalWrite(trigPin, HIGH);           // Set trigger
  delayMicroseconds(delayRadarTrig2);    // Wait
  digitalWrite(trigPin, LOW);            // Restore trigger, starts the ultrasound
  while(digitalRead(echoPin) != HIGH){}; // Wait for the pin to go HIGH, do nothing
  t0 = micros();                         // Start the clock
  duration = 0;                          // Set initial duration to zero
  while((duration<timeout) and (digitalRead(echoPin) == HIGH)) // Wait for the pin to go LOW until the timeout
  {
    t1 = micros();                       // Get the time
    duration = t1 - t0;                  // Update duration
  }
  // Calculate the distance (in cm) based on the speed of sound.
  distance = duration/58.3;
  if (duration>=timeout)
    distance=-1;
  // Send the distance to the computer using the serial port
  Serial.println(distance);
  delayMicroseconds(delayRadarWait);
}

void loop() {
  Measure();
}