#include <AFMotor.

h>

#include <Servo.h>

#include <delay.h>

#include <io.h>

#include <stdio.h>

#include <stdlib.h>

Servo myservo;

int ENABLE_A = 28;

int PIN_In1 = 38;

int PIN_In2 = 36;

int ENABLE_B = 33;

int PIN_In3 = 35;

int PIN_In4 = 34;

int SENSOR_DISTANCE;

int LEFT_SENSOR_DISTANCE;

int RIGHT_SENSOR_DISTANCE;

int SNZ_DISTANCE_L;

int SNZ_DISTANCE_R;

int LFT_SNZ_DIS;

int RGT_SNZ_DIS;

#define TRIG_PIN 40
#define ECHO_PIN 39

#define MIN_DISTANCE 40

#define MAX_DISTANCE 200

#define INTERVAL 200

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

void setup() {

pinMode (ENABLE_A, OUTPUT);

pinMode (PIN_In1, OUTPUT);

pinMode (PIN_In2, OUTPUT);

pinMode (ENABLE_B, OUTPUT);

pinMode (PIN_In3, OUTPUT);

pinMode (PIN_In4, OUTPUT);

pinMode(TRIG_PIN, OUTPUT);

pinMode(ECHO_PIN, INPUT);

myservo.attach(9);

sensorCenter();
 stopMotors();

void loop() {

SENSOR_DISTANCE=sensorDistance();

if(SENSOR_DISTANCE >= MIN_DISTANCE || SENSOR_DISTANCE==0)

goForward();

else {

stopMotors();

LFT_SNZ_DIS=toTheLeft();

RGT_SNZ_DIS=toTheRight();

if(LFT_SNZ_DIS >= MIN_DISTANCE && LFT_SNZ_DIS >= RGT_SNZ_DIS)

int NEW_SNZ_DIS_LFT;

for(int i=1;i<=3;i++){

goLeft();

delay(300);

stopMotors();
 NEW_SNZ_DIS_LFT=sensorDistance();

if(NEW_SNZ_DIS_LFT >=MIN_DISTANCE)

break;

sensorCenter();

else if(RGT_SNZ_DIS >= MIN_DISTANCE && RGT_SNZ_DIS >= LFT_SNZ_DIS)

int NEW_SNZ_DIS_RGT;

for(int j=1;j<=3;j++){

goRight();

delay(300);

stopMotors();

NEW_SNZ_DIS_RGT=sensorDistance();

if(NEW_SNZ_DIS_RGT >=MIN_DISTANCE)

break;

sensorCenter();

else
 {

goBackward();

delay(500);

stopMotors();

sensorCenter();

void stopMotors(){

analogWrite (ENABLE_A, 0);

analogWrite (ENABLE_B, 0);

digitalWrite (PIN_In1, LOW);

digitalWrite (PIN_In2, LOW);

digitalWrite (PIN_In3, LOW);

digitalWrite (PIN_In4, LOW);

int sensorDistance(){

int distance;

int uS = sonar.ping();

distance = uS / US_ROUNDTRIP_CM;

if(distance !=0){

return distance;

delay(300);
 }

void goForward(){

analogWrite (ENABLE_A, 255);

digitalWrite (PIN_In1, HIGH);

digitalWrite (PIN_In2, LOW);

analogWrite (ENABLE_B, 255);

digitalWrite (PIN_In3, HIGH);

digitalWrite (PIN_In4, LOW);

void goBackward(){

analogWrite (ENABLE_A, 180);

digitalWrite (PIN_In1, LOW);

digitalWrite (PIN_In2, HIGH);

analogWrite (ENABLE_B, 180);

digitalWrite (PIN_In3, LOW);

digitalWrite (PIN_In4, HIGH);

void goLeft(){

analogWrite (ENABLE_A, 180);

digitalWrite (PIN_In1, HIGH);

digitalWrite (PIN_In2, LOW);

analogWrite (ENABLE_B, 180);

 digitalWrite (PIN_In3, LOW);

digitalWrite (PIN_In4, HIGH);

void goRight(){

analogWrite (ENABLE_A, 180);

digitalWrite (PIN_In1, LOW);

digitalWrite (PIN_In2, HIGH);

analogWrite (ENABLE_B, 180);

digitalWrite (PIN_In3, HIGH);

digitalWrite (PIN_In4, LOW);

void sensorCenter(){

myservo.write(90);

delay(500);

void turnSensorLeft(){

myservo.write(120);

delay(500);

void turnSensorRight(){

myservo.write(60);

delay(500);
}

int leftSensorDistance(){

int LEFT_SENSOR_DISTANCE;

LEFT_SENSOR_DISTANCE=sensorDistance();

return LEFT_SENSOR_DISTANCE;

int rightSensorDistance(){

int RIGHT_SENSOR_DISTANCE;

RIGHT_SENSOR_DISTANCE=sensorDistance();

return RIGHT_SENSOR_DISTANCE;

int toTheLeft(){

int LEFT_SENSOR_DISTANCE;

turnSensorLeft();

LEFT_SENSOR_DISTANCE=leftSensorDistance();

return LEFT_SENSOR_DISTANCE;

int toTheRight(){

int RIGHT_SENSOR_DISTANCE;

turnSensorRight();

RIGHT_SENSOR_DISTANCE=rightSensorDistance();

return RIGHT_SENSOR_DISTANCE;