#include <AFMotor.h> //Adds Motor shield library
#define trigPin 7
#define echoPin 6
float v;
int duration;
long distance;
long distance1;
long distance2;
AF_DCMotor motor(1);
void setup() {
Serial.begin(9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
motor.setSpeed(200);
motor.run(RELEASE);
}
void loop() {
do{
distance = (duration/2) /29.1;
digitalWrite(trigPin, HIGH);
delayMicroseconds(1000);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) /29.1;
Serial.print("distance ");
Serial.print(distance);
Serial.println("cm");
}while(distance <= 4 || distance > 40);
do{
digitalWrite(trigPin, HIGH);
delayMicroseconds(1000);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance1 = (duration/2) /29.1;
Serial.print("distance1 loop ");
Serial.print(distance1);
Serial.println("cm");
delay(100);
v = ((float)distance1/(distance+1))*255;
Serial.print("speed ");
Serial.println(v);
motor.run(FORWARD);
motor.setSpeed(v);
Serial.println("Forward");
}while(distance1 > 5);
motor.run(RELEASE);
motor.setSpeed(0);
delay(3000);
do{
digitalWrite(trigPin, HIGH);
delayMicroseconds(1000);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance2 = (duration/2) /29.1;
Serial.print("distance2 loop 2 ");
Serial.print(distance2);
Serial.println("cm");
Serial.println("Backward");
motor.run(BACKWARD);
motor.setSpeed(150);
}while(distance2 <= 19);
motor.run(RELEASE);
delay(3000);
}