1#include <AFMotor.h>
2
3#define trigPin 7
4#define echoPin 6
5
6float v;
7int duration;
8long distance;
9long distance1;
10long distance2;
11
12AF_DCMotor motor(1);
13
14void setup() {
15 Serial.begin(9600);
16 pinMode(trigPin, OUTPUT);
17 pinMode(echoPin, INPUT);
18
19 motor.setSpeed(200);
20 motor.run(RELEASE);
21}
22
23void loop() {
24 do{
25 distance = (duration/2) /29.1;
26 digitalWrite(trigPin, HIGH);
27 delayMicroseconds(1000);
28 digitalWrite(trigPin, LOW);
29 duration = pulseIn(echoPin, HIGH);
30 distance = (duration/2) /29.1;
31 Serial.print("distance ");
32 Serial.print(distance);
33 Serial.println("cm");
34 }while(distance <= 4 || distance > 40);
35
36 do{
37 digitalWrite(trigPin, HIGH);
38 delayMicroseconds(1000);
39 digitalWrite(trigPin, LOW);
40 duration = pulseIn(echoPin, HIGH);
41 distance1 = (duration/2) /29.1;
42 Serial.print("distance1 loop ");
43 Serial.print(distance1);
44 Serial.println("cm");
45 delay(100);
46 v = ((float)distance1/(distance+1))*255;
47 Serial.print("speed ");
48 Serial.println(v);
49 motor.run(FORWARD);
50 motor.setSpeed(v);
51 Serial.println("Forward");
52 }while(distance1 > 5);
53
54motor.run(RELEASE);
55motor.setSpeed(0);
56delay(3000);
57
58do{
59 digitalWrite(trigPin, HIGH);
60 delayMicroseconds(1000);
61 digitalWrite(trigPin, LOW);
62 duration = pulseIn(echoPin, HIGH);
63 distance2 = (duration/2) /29.1;
64 Serial.print("distance2 loop 2 ");
65 Serial.print(distance2);
66 Serial.println("cm");
67 Serial.println("Backward");
68 motor.run(BACKWARD);
69 motor.setSpeed(150);
70}while(distance2 <= 19);
71motor.run(RELEASE);
72delay(3000);
73}
