/*
  Copyright (C) 2024, Pablo César Galdo Regueiro.
  info.wokwi(at)pcgaldo.com

  Project in editing process. Operation may be limited.

  License

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <https://www.gnu.org/licenses/>
*/

#define trigPinLeft 2
#define echoPinLeft 3
#define trigPinRight 4
#define echoPinRight 5
#define relayPin 6

const float durationToDistanceFactor = 58.0;

void setup() {
  Serial.begin(115200);

  // Set pin modes for trigger and echo pins
  pinMode(trigPinLeft, OUTPUT);
  pinMode(echoPinLeft, INPUT);
  pinMode(trigPinRight, OUTPUT);
  pinMode(echoPinRight, INPUT);
  
  // Set pin mode for relay
  pinMode(relayPin, OUTPUT);

  // Initialize relay to be off
  digitalWrite(relayPin, LOW);
}

long measureDistance(int trigPin, int echoPin) {
  // Send a 10 microsecond pulse to trigger the sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the duration of the echo pulse
  long duration = pulseIn(echoPin, HIGH);
  
  // Convert duration to distance in centimeters
  long distance = duration / durationToDistanceFactor;
  return distance;
}

void loop() {
  // Measure distances from both sensors
  long distanceLeft = measureDistance(trigPinLeft, echoPinLeft);
  long distanceRight = measureDistance(trigPinRight, echoPinRight);

  // Print measured distances to the serial monitor
  Serial.print("Left distance in cm: ");
  Serial.println(distanceLeft);
  Serial.print("Right distance in cm: ");
  Serial.println(distanceRight);

  // Check if the right distance is less than or equal to the left distance
  if (distanceRight <= distanceLeft) {
    digitalWrite(relayPin, HIGH); // Activate the relay
    Serial.println("Object centered or to the right, relay activated.");
  } else {
    digitalWrite(relayPin, LOW); // Deactivate the relay
    Serial.println("Object not centered, relay deactivated.");
  }

  // Wait for 1 second before taking the next measurement
  //delay(1000);
}