Arduino Line Follower Robot with Obstacle Avoidance

1#include <NewPing.h>
2#include <Servo.h>
3#include <AFMotor.h>
4
5//hc-sr04 sensor
6#define TRIGGER_PIN A2
7#define ECHO_PIN A3
8#define max_distance 50
9
10//ir sensor
11#define irLeft A0
12#define irRight A1
13
14//motor
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    //forword
50  }
51  else if (digitalRead(irLeft) == 0 && digitalRead(irRight) == 1 ) {
52    objectAvoid();
53    Serial.println("TL");
54    //leftturn
55    moveLeft();
56  }
57  else if (digitalRead(irLeft) == 1 && digitalRead(irRight) == 0 ) {
58    objectAvoid();
59    Serial.println("TR");
60    //rightturn
61    moveRight();
62  }
63  else if (digitalRead(irLeft) == 1 && digitalRead(irRight) == 1 ) {
64    //Stop
65    Stop();
66  }
67}
68
69void objectAvoid() {
70  distance = getDistance();
71  if (distance <= 15) {
72    //stop
73    Stop();
74    Serial.println("Stop");
75
76    lookLeft();
77    lookRight();
78    delay(100);
79    if (rightDistance <= leftDistance) {
80      //left
81      object = true;
82      turn();
83      Serial.println("moveLeft");
84    } else {
85      //right
86      object = false;
87      turn();
88      Serial.println("moveRight");
89    }
90    delay(100);
91  }
92  else {
93    //forword
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  //lock left
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  //lock right
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}