// L298N - Version: Latest
#include <L298N.h>
const unsigned int IN1 = 22;
const unsigned int IN2 = 23;
const unsigned int ENA = 13;

const unsigned int IN3 = 24;
const unsigned int IN4 = 25;
const unsigned int ENB = 12;

// Create one motor instance
L298N motor1(ENA, IN1, IN2);
L298N motor2(ENB, IN3, IN4);
#define trigPin 3
#define echoPin 2


void setup() {
  Serial.begin(9600); //Begin serial communication
  Serial.println("Motor test!");
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  motor1.setSpeed(105);
  motor2.setSpeed(105);


}

// left motor is in1 and in2
// right motor is in3 and in4

void loop() {

    digitalWrite (trigPin, HIGH);
delay(50);
digitalWrite (trigPin, LOW);
int duration = pulseIn(echoPin, HIGH);
int distance=(duration/2)/29.1;
Serial.println(distance);

if (distance < 25) {
Serial.println("Obstacle detected");
  Serial.println("Commencing manuever");
    motor1.stop();
    motor2.stop();
    delay(750);

    motor1.backward();
    motor2.backward();
    delay(500);

    motor1.setSpeed(140);
    motor2.setSpeed(140);
  motor1.forward();
  delay(450);

}

  else {
    Serial.println("No obstacles ahead. Proceeding current route");
    delay(15);
    motor1.setSpeed(200);
    motor2.setSpeed(200);
    motor1.forward();
  motor2.forward();

  }


  }