1#include <AFMotor.h>
2
3const int trigPin = A0;
4const int echoPin = A2;
5long duration;
6int distance;
7
8AF_DCMotor motor1(1, MOTOR12_1KHZ);
9AF_DCMotor motor2(2, MOTOR12_1KHZ);
10AF_DCMotor motor3(3, MOTOR12_1KHZ);
11AF_DCMotor motor4(4, MOTOR12_1KHZ);
12
13
14
15void setup() {
16 pinMode(trigPin, OUTPUT);
17 pinMode(echoPin, INPUT);
18
19 Serial.begin(9600);
20
21 motor1.setSpeed(250);
22 motor2.setSpeed(250);
23 motor3.setSpeed(250);
24 motor4.setSpeed(250);
25
26}
27
28void loop()
29{
30
31digitalWrite(trigPin, LOW);
32delayMicroseconds(2);
33digitalWrite(trigPin, HIGH);
34delayMicroseconds(10);
35digitalWrite(trigPin, LOW);
36duration = pulseIn(echoPin, HIGH);
37distance= duration*0.034/2;
38Serial.print("Distance: ");
39Serial.println(distance);
40
41movestop();
42delay(1000);
43
44if(distance < 75)
45{
46
47 backward();
48 delay(1000);
49
50
51 movestop();
52 delay(1000);
53
54 lookright();
55 delay(750);
56}
57else
58{
59 forward();
60 delay(1000);
61
62}
63
64
65}
66
67
68void movestop()
69{
70 motor1.run(RELEASE);
71 motor2.run(RELEASE);
72 motor3.run(RELEASE);
73 motor4.run(RELEASE);
74}
75
76void forward()
77{
78 motor1.run(FORWARD);
79 motor2.run(FORWARD);
80 motor3.run(FORWARD);
81 motor4.run(FORWARD);
82 motor1.setSpeed(200);
83 motor2.setSpeed(200);
84 motor3.setSpeed(200);
85 motor4.setSpeed(200);
86
87}
88
89void backward()
90{
91 motor1.run(BACKWARD);
92 motor2.run(BACKWARD);
93 motor3.run(BACKWARD);
94 motor4.run(BACKWARD);
95 motor1.setSpeed(200);
96 motor2.setSpeed(200);
97 motor3.setSpeed(200);
98 motor4.setSpeed(200);
99
100}
101
102void lookright()
103{
104 motor1.run(FORWARD);
105 motor2.run(FORWARD);
106 motor3.run(BACKWARD);
107 motor4.run(BACKWARD);
108 motor1.setSpeed(200);
109 motor2.setSpeed(200);
110 motor3.setSpeed(200);
111 motor4.setSpeed(200);
112
113}
114
115void lookleft()
116{
117 motor1.run(BACKWARD);
118 motor2.run(BACKWARD);
119 motor3.run(FORWARD);
120 motor4.run(FORWARD);
121 motor1.setSpeed(200);
122 motor2.setSpeed(200);
123 motor3.setSpeed(200);
124 motor4.setSpeed(200);
125
126}
127
darshdrone
3 years ago
but how do you insert a ultrasonic sensor??