Selsa

#include <AFMotor.h>

int trigPin = A2;
int echoPin = A0;
int delaytime = 200;
float duration;
float distance;

float soundVelocity = 343;

AF_DCMotor motor1(1, MOTOR12_64KHZ);
AF_DCMotor motor2(2, MOTOR12_64KHZ);



void setup() {
  AF_DCMotor begin();
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void forward()
{
  motor1.setSpeed(255);
  motor2.setSpeed(255);
  motor1.run(FORWARD);
  motor2.run(FORWARD);
}

void backward()
{
  motor1.setSpeed(255);
  motor1.run(BACKWARD);
  motor2.setSpeed(255);
  motor2.run(BACKWARD);
}

void move_stop()
{
  motor1.run(RELEASE);
  motor2.run(RELEASE);
}

void turn()
{
  motor1.setSpeed(255);
  motor1.run(BACKWARD);
  motor2.setSpeed(255);
  motor2.run(FORWARD);

  Serial.begin(9600);
}

void loop() {

  

  digitalWrite(trigPin, LOW);
  delayMicroseconds(10);

  digitalWrite(trigPin, HIGH);
  delay(delaytime);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  duration = duration / 1e+6; // to change the second from microsecond

  duration = duration/2; //to get the duration for one time

  distance = soundVelocity * duration;

  float final_distance = distance * 10; // to get the centimeter

  Serial.println(final_distance);


  
  if (final_distance < 5)
  {
    move_stop();
    delay(10);
    backward();
    delay(500);
    turn();
    delay(1000);
    final_distance = 0;
  }

  else
  {
    forward();
    delay(90);
  }
}