/* 
 * Pure ranging application of the ultrasound sensor.
 * Daniel Sjoberg, 2020-01-26
 */

#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

// Timing parameters given in us
int delayRadarTrig1 = 10;                // Delay after first trig LOW
int delayRadarTrig2 = 10;                // Delay after second trig HIGH
unsigned long timegate = 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 
  unsigned long duration=0;              // Time that we have been waiting for echo
  unsigned long target_delay=0;          // Time delay to target
  float distance=0;                      // Distance in cm to be output
  
  
  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
  target_delay = 0;                      // Set initial target delay negative
  while(duration < timegate)             // Wait until the timegate is passed
  {
    t1 = micros();                       // Get the time
    duration = t1 - t0;                  // Update duration
    if ((digitalRead(echoPin) == LOW) and (target_delay == 0))
      target_delay = duration;
  }
  // Calculate the distance (in cm) based on the speed of sound.
  distance = target_delay/58.3;
  if (target_delay == 0)
    distance = -1;



  // Send the distance to the computer using the serial port
  Serial.println(distance);
}

void loop() {
  Measure();
}