1. // IR Sensors
2. int sensor1 = A0;      // Leftmost sensor
3. int sensor2 = A1;      // Left sensor
4. int sensor3 = A2;      // Middle sensor
5. int sensor4 = A3;      // Right sensor
6. int sensor5 = A4;      // Rightmost sensor
7. int sensor6 = A5;      //
8.  
9. int sensor[5] = {0, 0, 0, 0, 0,0};
10.  
11. // Motor Variables
12. int motorInput1 = 7;   // AIN1
13. int motorInput2 = 8;   // AIN2
14. int motorInput3 = 5;   // BIN1
15. int motorInput4 = 4;   // BIN2
16. int PWMA = 9;
17. int PWMB = 3;
18.  
19. // Initial Speed of Motor
20. int initial_motor_speed = 255;
21.  
22. // PID Constants
23. float Kp = 10;
24. float Ki = 0;
25. float Kd = 200;
26.  
27. float error = 0, P = 0, I = 0, D = 0, PID_value = 0;
28. float previous_error = 0, previous_I = 0;
29.  
30. void setup()
31. {
32.   pinMode(sensor1, INPUT);
33.   pinMode(sensor2, INPUT);
34.   pinMode(sensor3, INPUT);
35.   pinMode(sensor4, INPUT);
36.   pinMode(sensor5, INPUT);
37.  
38.   pinMode(motorInput1, OUTPUT);
39.   pinMode(motorInput2, OUTPUT);
40.   pinMode(motorInput3, OUTPUT);
41.   pinMode(motorInput4, OUTPUT);
42.   pinMode(PWMA, OUTPUT);
43.   pinMode(PWMB, OUTPUT);
44.  
45.   Serial.begin(9600);
46.   Serial.println("Started !!");
47. }
48.  
49. void loop()
50. {
51.   read_sensor_values();
52.  
53.   if (error == 100 || error == 105) {
54.     do {
55.       analogWrite(PWMA, 100); // Left Motor Speed
56.       analogWrite(PWMB, 255); // Right Motor Speed
57.       sharpLeftTurn();
58.       read_sensor_values();
59.       if (error == 0) {
60.         stop_bot();
61.         delay(200);
62.       }
63.     } while (error != 0);
64.   }  else if (error == 101 || error == 104) {
65.     analogWrite(PWMA, 255); // Left Motor Speed
66.     analogWrite(PWMB, 100); // Right Motor Speed
67.     forward();
68.     delay(200);
69.  
70.     stop_bot();
71.     read_sensor_values();
72.     if (error == 102) {
73.       do {
74.         analogWrite(PWMA, 169); // Left Motor Speed
75.         analogWrite(PWMB, 132); // Right Motor Speed
76.         sharpRightTurn();
77.         read_sensor_values();
78.       } while (error != 0);
79.     }
80.   } else if (error == 102) {
81.     do {
82.       analogWrite(PWMA, 169); // Left Motor Speed
83.       analogWrite(PWMB, 132); // Right Motor Speed
84.       sharpLeftTurn();
85.       read_sensor_values();
86.       if (error == 0) {
87.         stop_bot();
88.         delay(200);
89.       }
90.     } while (error != 0);
91.   } else if (error == 103) {
92.     analogWrite(PWMA, 100); // Left Motor Speed
93.     analogWrite(PWMB, 255); // Right Motor Speed
94.     forward();
95.     delay(200);
96.     read_sensor_values();
97.   }
98.   else {
99.     calculate_pid();
100.     motor_control();
101.   }
102. }
103.  
104. void read_sensor_values()
105. {
106.   sensor[0] = !digitalRead(sensor1);
107.   sensor[1] = !digitalRead(sensor2);
108.   sensor[2] = !digitalRead(sensor3);
109.   sensor[3] = !digitalRead(sensor4);
110.   sensor[4] = !digitalRead(sensor5);
111.   sensor[5] = !digitalRead(sensor6);
112.  
113.  if      
114.       ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) (sensor[5] == 0)
115.     error = -80;
116.   else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) && (sensor[5] == 0)
117.     error = -70;
118.   else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) && (sensor[5] == 0)
119.     error = -60;
120.   else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) && (sensor[5] == 0)
121.     error = 0;
122.   else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) && (sensor[5] == 0)
123.     error = 60;
124.   else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) && (sensor[5] == 0)
125.     error = 0;
126.   else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) && (sensor[5] == 0)
127.     error = 70;
128.   else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[3] == 1) && (sensor[4] == 0) && (sensor[2] == 1)) && (sensor[5] == 0) // Turn robot left side
129.     error = 100;
130.   else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[3] == 1) && (sensor[4] == 1) && (sensor[2] == 1)) && (sensor[5] == 1) // Turn robot right side
131.     error = 101;
132.   else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[3] == 0) && (sensor[4] == 0) && (sensor[2] == 0)) && (sensor[5] == 0) // Make U turn
133.     error = 102;
134.   else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[3] == 1) && (sensor[4] == 0) && (sensor[2] == 1)) && (sensor[5] == 0) // Turn left side or stop
135.     error = 103;
136.   else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[3] == 1) && (sensor[4] == 1) && (sensor[2] == 1)) && (sensor[5] == 1) // Turn right side or stop
137.     error = 104;
138.   else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[3] == 1) && (sensor[4] == 0) && (sensor[2] == 0)) && (sensor[5] == 0) // Turn left side or stop
139.     error = 105;
140. }
141. }
142.  
143. void calculate_pid()
144. {
145.   P = error;
146.   I = I + previous_I;
147.   D = error - previous_error;
148.  
149.   PID_value = (Kp * P) + (Ki * I) + (Kd * D);
150.  
151.   previous_I = I;
152.   previous_error = error;
153. }
154.  
155. void motor_control()
156. {
157.   int left_motor_speed = initial_motor_speed - PID_value + 30;
158.   int right_motor_speed = initial_motor_speed + PID_value - 39;
159.  
160.   left_motor_speed = constrain(left_motor_speed, -255, 255);
161.   right_motor_speed = constrain(right_motor_speed, -255, 255);
162.  
163.   Serial.print(" | Error: ");
164.   Serial.print(error);
165.   Serial.print(" | PID: ");
166.   Serial.print(PID_value);
167.   Serial.print(" | Right Speed: ");
168.   Serial.print(left_motor_speed);
169.   Serial.print(" | Left Speed: ");
170.   Serial.println(right_motor_speed);
171.  
172.   analogWrite(PWMB, left_motor_speed);
173.   analogWrite(PWMA, right_motor_speed);
174.  
175.   forward();
176. }
177.  
178. void forward()
179. {
180.   digitalWrite(motorInput1, HIGH);
181.   digitalWrite(motorInput2, LOW);
182.   digitalWrite(motorInput3, HIGH);
183.   digitalWrite(motorInput4, LOW);
184. }
185.  
186. void reverse()
187. {
188.   digitalWrite(motorInput1, LOW);
189.   digitalWrite(motorInput2, HIGH);
190.   digitalWrite(motorInput3, LOW);
191.   digitalWrite(motorInput4, HIGH);
192. }
193.  
194. void right()
195. {
196.   digitalWrite(motorInput1, HIGH);
197.   digitalWrite(motorInput2, LOW);
198.   digitalWrite(motorInput3, HIGH);
199.   digitalWrite(motorInput4, HIGH);
200. }
201.  
202. void left()
203. {
204.   digitalWrite(motorInput1, HIGH);
205.   digitalWrite(motorInput2, HIGH);
206.   digitalWrite(motorInput3, HIGH);
207.   digitalWrite(motorInput4, LOW);
208. }
209.  
210. void sharpRightTurn() {
211.   digitalWrite(motorInput1, HIGH);
212.   digitalWrite(motorInput2, LOW);
213.   digitalWrite(motorInput3, LOW);
214.   digitalWrite(motorInput4, HIGH);
215. }
216.  
217. void sharpLeftTurn() {
218.   digitalWrite(motorInput1, LOW);
219.   digitalWrite(motorInput2, HIGH);
220.   digitalWrite(motorInput3, HIGH);
221.   digitalWrite(motorInput4, LOW);
222. }
223.  
224. void stop_bot()
225. {
226.   digitalWrite(motorInput1, HIGH);
227.   digitalWrite(motorInput2, HIGH);
228.   digitalWrite(motorInput3, HIGH);
229.   digitalWrite(motorInput4, HIGH);
230. }