1#include <NewPing.h>
2#include <Servo.h>
3#include <AFMotor.h>
4
5
6#define TRIGGER_PIN A2
7#define ECHO_PIN A3
8#define max_distance 50
9
10
11#define irLeft A0
12#define irRight A1
13
14
15#define MAX_SPEED 200
16#define MAX_SPEED_OFFSET 20
17
18Servo servo;
19
20NewPing sonar(TRIGGER_PIN, ECHO_PIN, max_distance);
21
22AF_DCMotor motor1(1, MOTOR12_1KHZ);
23AF_DCMotor motor2(2, MOTOR12_1KHZ);
24AF_DCMotor motor3(3, MOTOR34_1KHZ);
25AF_DCMotor motor4(4, MOTOR34_1KHZ);
26
27
28int distance = 0;
29int leftDistance;
30int rightDistance;
31boolean object;
32
33void setup() {
34 Serial.begin(9600);
35 pinMode(irLeft, INPUT);
36 pinMode(irRight, INPUT);
37 servo.attach(10);
38 servo.write(90);
39
40 motor1.setSpeed(120);
41 motor2.setSpeed(120);
42 motor3.setSpeed(120);
43 motor4.setSpeed(120);
44}
45
46void loop() {
47 if (digitalRead(irLeft) == 0 && digitalRead(irRight) == 0 ) {
48 objectAvoid();
49
50 }
51 else if (digitalRead(irLeft) == 0 && digitalRead(irRight) == 1 ) {
52 objectAvoid();
53 Serial.println("TL");
54
55 moveLeft();
56 }
57 else if (digitalRead(irLeft) == 1 && digitalRead(irRight) == 0 ) {
58 objectAvoid();
59 Serial.println("TR");
60
61 moveRight();
62 }
63 else if (digitalRead(irLeft) == 1 && digitalRead(irRight) == 1 ) {
64
65 Stop();
66 }
67}
68
69void objectAvoid() {
70 distance = getDistance();
71 if (distance <= 15) {
72
73 Stop();
74 Serial.println("Stop");
75
76 lookLeft();
77 lookRight();
78 delay(100);
79 if (rightDistance <= leftDistance) {
80
81 object = true;
82 turn();
83 Serial.println("moveLeft");
84 } else {
85
86 object = false;
87 turn();
88 Serial.println("moveRight");
89 }
90 delay(100);
91 }
92 else {
93
94 Serial.println("moveforword");
95 moveForward();
96 }
97}
98
99int getDistance() {
100 delay(50);
101 int cm = sonar.ping_cm();
102 if (cm == 0) {
103 cm = 100;
104 }
105 return cm;
106}
107
108int lookLeft () {
109
110 servo.write(150);
111 delay(500);
112 leftDistance = getDistance();
113 delay(100);
114 servo.write(90);
115 Serial.print("Left:");
116 Serial.print(leftDistance);
117 return leftDistance;
118 delay(100);
119}
120
121int lookRight() {
122
123 servo.write(30);
124 delay(500);
125 rightDistance = getDistance();
126 delay(100);
127 servo.write(90);
128 Serial.print(" ");
129 Serial.print("Right:");
130 Serial.println(rightDistance);
131 return rightDistance;
132 delay(100);
133}
134void Stop() {
135 motor1.run(RELEASE);
136 motor2.run(RELEASE);
137 motor3.run(RELEASE);
138 motor4.run(RELEASE);
139}
140void moveForward() {
141 motor1.run(FORWARD);
142 motor2.run(FORWARD);
143 motor3.run(FORWARD);
144 motor4.run(FORWARD);
145}
146void moveBackward() {
147 motor1.run(BACKWARD);
148 motor2.run(BACKWARD);
149 motor3.run(BACKWARD);
150 motor4.run(BACKWARD);
151
152}
153void turn() {
154 if (object == false) {
155 Serial.println("turn Right");
156 moveLeft();
157 delay(700);
158 moveForward();
159 delay(800);
160 moveRight();
161 delay(900);
162 if (digitalRead(irRight) == 1) {
163 loop();
164 } else {
165 moveForward();
166 }
167 }
168 else {
169 Serial.println("turn left");
170 moveRight();
171 delay(700);
172 moveForward();
173 delay(800);
174 moveLeft();
175 delay(900);
176 if (digitalRead(irLeft) == 1) {
177 loop();
178 } else {
179 moveForward();
180 }
181 }
182}
183void moveRight() {
184 motor1.run(BACKWARD);
185 motor2.run(BACKWARD);
186 motor3.run(FORWARD);
187 motor4.run(FORWARD);
188}
189void moveLeft() {
190 motor1.run(FORWARD);
191 motor2.run(FORWARD);
192 motor3.run(BACKWARD);
193 motor4.run(BACKWARD);
194}