/*
  Arduino | sensors-help
  Chiara — Today at 3:46 PM
  thank you!! I'm lost as to how to then connect all these
  elements (sensors, motors driver...) togheter.
  Do you have any links to similar projects or some guide
  on how to start doing all this? is there an arduino kit
  you suggest buying or should i look for individual pieces?
  any info is helpful!
*/

const int THRESHOLD = 50;
// pin constants
const int ECHO_1_PIN = 6;
const int ECHO_2_PIN = 2;
const int TRIG_1_PIN = 7;
const int TRIG_2_PIN = 3;
const int LEFT_LED_PIN = 5;
const int RIGHT_LED_PIN = 4;

long duration = 0;
int distance = 0;

int calculateDistance(long duration)  {
  // Convert the time into a distance
  int cm = int((duration / 2) / 29.1);   // Divide by 29.1 or multiply by 0.0343
  return cm;
}

void displayDistance(int sensor, int reading) {
  Serial.print("Sensor ");
  Serial.print(sensor);
  Serial.print(":\t");
  Serial.print(reading);
  Serial.println("cm");

  if (sensor == 1)  {
    if (reading <= THRESHOLD) {
      digitalWrite(LEFT_LED_PIN, HIGH);
    } else {
      digitalWrite(LEFT_LED_PIN, LOW);
    }
  }
  if (sensor == 2)  {
    if (reading <= THRESHOLD) {
      digitalWrite(RIGHT_LED_PIN, HIGH);
    } else {
      digitalWrite(RIGHT_LED_PIN, LOW);
    }
  }
}

void setup() {
  Serial.begin(9600);
  pinMode(ECHO_1_PIN, INPUT);
  pinMode(ECHO_2_PIN, INPUT);
  pinMode(TRIG_1_PIN, OUTPUT);
  pinMode(TRIG_2_PIN, OUTPUT);
  pinMode(LEFT_LED_PIN, OUTPUT);
  pinMode(RIGHT_LED_PIN, OUTPUT);
}

void loop() {
  // read senor 1
  // send "ping"
  digitalWrite(TRIG_1_PIN, LOW);
  delayMicroseconds(5);
  digitalWrite(TRIG_1_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_1_PIN, LOW);
  // listen for echo
  duration = pulseIn(ECHO_1_PIN, HIGH);
  distance = calculateDistance(duration);
  displayDistance(1, distance);
  // read senor 2
  // send "ping"
  digitalWrite(TRIG_2_PIN, LOW);
  delayMicroseconds(5);
  digitalWrite(TRIG_2_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_2_PIN, LOW);
  // listen for echo
  duration = pulseIn(ECHO_2_PIN, HIGH);
  distance = calculateDistance(duration);
  displayDistance(2, distance);
  Serial.println();

  delay(1000);
}