///////////////////////// AdaFruit Sheild Definitions //////////////////////////////////////////////////////
// URL : https://lastminuteengineers.com/l293d-motor-driver-shield-arduino-tutorial/

// Include Used Lib (1st Install LIBs from LIB Manager)
#include <AFMotor.h>    // Install LIB (Adafruit Motor Shield library)

// Define Which Motor Will be Used
AF_DCMotor motor1(1);   // define 1st Motor
AF_DCMotor motor2(2);   // define 2nd Motor

////////////////////// Define Servo //////////////////////////////////////////////////////////////////////
#include <Servo.h>      // Install LIB (Servo)

Servo servo1;          // create servo object to control a servo
int servoPos1 = 0;
int servoDir1 = 1;
int isIncreasing = 1;

//////////////////////////////////////////////



#define PIN_TRIG 3
#define PIN_ECHO 2

int oldDist = 0;
int direction = 0;
int distance = 0;

void setup() {
  motor1.run(RELEASE);  // RELEASE >> Cut Power // FORWARD // BACKWARD
  motor2.run(RELEASE);  // RELEASE >> Cut Power // FORWARD // BACKWARD  
  
  servoPos1 = 90;
  // attaches the servo on pin 10 to the servo object
  servo1.attach(10); 
  servo1.write(servoPos1);

  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  pinMode(6, OUTPUT);
  pinMode(10, OUTPUT);
  analogWrite(6,LOW);
  analogWrite(10,LOW);

  Serial.begin(115200);
}

void loop() {
  oldDist = 0;
  direction = 0;
  distance = 0;
  for (int sa = 0; sa <= 180; sa++) {
    servo1.write(sa);
    // Start a new measurement:
    digitalWrite(PIN_TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(PIN_TRIG, LOW);
    // Read the result:
    int duration = pulseIn(PIN_ECHO, HIGH);
    distance = duration / 58;
    if (distance > oldDist) {
      oldDist = distance;
      direction = sa;
    // Serial.println(direction);
    }
    delay(5);
  }
  // Serial.println(direction);
  servo1.write(90);
  analogWrite(6,LOW);
  analogWrite(10,LOW);
  if (direction < 90) {
    // Move to the LEFT
    analogWrite(6,0);
    analogWrite(10,255);
  }
  if (direction > 90) {
    // Move to the RIGHT
    analogWrite(6,255);
    analogWrite(10,0);
  }
  if (direction == 90) {
    // Move FORWORD
    analogWrite(6,255);
    analogWrite(10,255);
  }
  delay(10);

}