#include <AFMotor.h>
const int stepsPerRevolution = 2048 ; 
AF_Stepper motor(stepsPerRevolution, 2); 
#define echopin A1
#define trigpin A0 
const int MOTOR_1 = 1; 
const int MOTOR_2 = 2; 
AF_DCMotor motor1(MOTOR_1, MOTOR12_64KHZ); 
AF_DCMotor motor2(MOTOR_2, MOTOR12_64KHZ); 
int distance_L, distance_F, distance_R;
long distance;
int set = 20;
void setup() {
Serial.begin(9600);    
motor.setSpeed(0);
pinMode(trigpin, OUTPUT);
pinMode (echopin, INPUT );
motor1.setSpeed(150);          
motor2.setSpeed(150);
}
void loop() {
distance_F = data();
Serial.print("S=");
Serial.println(distance_F);
if (distance_F > set){
Serial.println("Forward");
motor1.run(FORWARD);         
motor2.run(FORWARD); 
}
else{hc_sr4();}
}
long data(){
digitalWrite(trigpin, LOW);
delayMicroseconds(2);
digitalWrite(trigpin, HIGH);
delayMicroseconds(10);
distance = pulseIn (echopin, HIGH);
return distance / 29 / 2;
}
void compareDistance(){
if (distance_L > distance_R){
motor1.run(FORWARD);   
motor2.run(BACKWARD);
delay(350);
}
else if (distance_R > distance_L){
motor1.run(BACKWARD);  
motor2.run(FORWARD); 
delay(350);
}
else{
motor1.run(BACKWARD);  
motor2.run(BACKWARD);
delay(300);
motor1.run(BACKWARD);   
motor2.run(FORWARD);
delay(500);                                                                                           }
}
void hc_sr4(){
Serial.println("Stop");
motor1.run(RELEASE);        
motor2.run(RELEASE);
motor.setSpeed(10); // 10 rpm 
motor.step(512, BACKWARD, MICROSTEP);
delay(300);
distance_R = data();
delay(100);
motor.setSpeed(10); // 10 rpm 
motor.step(1024, FORWARD, MICROSTEP ); 
delay(500);                                                                                                          
distance_L = data();                                                                                                                delay(100);
motor.setSpeed(10); // 10 rpm 
motor.step(512, BACKWARD, MICROSTEP);
delay(300);
compareDistance();
}