// =============================
// ESP32 2 Sensor Line Follower
// Sensors Outside the Black Line
// =============================


// IR Sensor Pins
#define LEFT_SENSOR  34
#define RIGHT_SENSOR 35


// Motor Pins
#define motor1A 26
#define motor1B 27
#define motor2A 14
#define motor2B 12


void setup()
{
  pinMode(LEFT_SENSOR, INPUT);
  pinMode(RIGHT_SENSOR, INPUT);


  pinMode(motor1A, OUTPUT);
  pinMode(motor1B, OUTPUT);
  pinMode(motor2A, OUTPUT);
  pinMode(motor2B, OUTPUT);
}


void loop()
{
  int left = digitalRead(LEFT_SENSOR);
  int right = digitalRead(RIGHT_SENSOR);


  // Both sensors on WHITE
  // Black line is between sensors
  if (left == HIGH && right == HIGH)
  {
    forward();
  }


  // Left sensor detects BLACK
  else if (left == LOW && right == HIGH)
  {
    turnLeft();
  }


  // Right sensor detects BLACK
  else if (left == HIGH && right == LOW)
  {
    turnRight();
  }


  // Both sensors detect BLACK
  else if (left == LOW && right == LOW)
  {
    forward();      // Change to stopRobot(); if preferred
  }
}


// =============================
// Motor Functions
// =============================


void forward()
{
  digitalWrite(motor1A, HIGH);
  digitalWrite(motor1B, LOW);


  digitalWrite(motor2A, HIGH);
  digitalWrite(motor2B, LOW);
}


void turnLeft()
{
  // Left motor Stop
  digitalWrite(motor1A, LOW);
  digitalWrite(motor1B, LOW);


  // Right motor Forward
  digitalWrite(motor2A, HIGH);
  digitalWrite(motor2B, LOW);
}


void turnRight()
{
  // Left motor Forward
  digitalWrite(motor1A, HIGH);
  digitalWrite(motor1B, LOW);


  // Right motor Stop
  digitalWrite(motor2A, LOW);
  digitalWrite(motor2B, LOW);
}


void stopRobot()
{
  digitalWrite(motor1A, LOW);
  digitalWrite(motor1B, LOW);


  digitalWrite(motor2A, LOW);
  digitalWrite(motor2B, LOW);
}