Arduino Uno based RoboTrack

1#include <SoftwareSerial.h>
2// ---------------- ULTRASONIC ----------------
3#define TRIG_PIN 9
4#define ECHO_PIN 10
5// ---------------- L298N ----------------
6#define ENA 5
7#define IN1 6
8#define IN2 7
9#define IN3 8
10#define IN4 11
11#define ENB 3
12// ---------------- BLUETOOTH ----------------
13// HC-05 TX -> Arduino D2
14// HC-05 RX -> Arduino D12
15SoftwareSerial BT(2, 12);
16// Bluetooth device name
17String bluetoothName = "HC-05";
18// ---------------- SETTINGS ----------------
19int motorSpeed = 160;
20int safeDistance = 20;
21// AUTO = obstacle avoidance
22// MANUAL = app control
23char mode = 'A';
24char command = 'S';
25// GET DISTANCE
26int getDistance() {
27  digitalWrite(TRIG_PIN, LOW);
28  delayMicroseconds(2);
29  digitalWrite(TRIG_PIN, HIGH);
30  delayMicroseconds(10);
31  digitalWrite(TRIG_PIN, LOW);
32  long duration = pulseIn(ECHO_PIN, HIGH, 30000);
33  if (duration == 0) {
34    return 400;
35  }
36  int distance = duration * 0.0343 / 2;
37  return distance;
38}
39// MOVE FORWARD
40void moveForward() {
41  digitalWrite(IN1, HIGH);
42  digitalWrite(IN2, LOW);
43  digitalWrite(IN3, HIGH);
44  digitalWrite(IN4, LOW);
45  analogWrite(ENA, motorSpeed);
46  analogWrite(ENB, motorSpeed);
47}
48// MOVE BACKWARD
49void moveBackward() {
50  digitalWrite(IN1, LOW);
51  digitalWrite(IN2, HIGH);
52  digitalWrite(IN3, LOW);
53  digitalWrite(IN4, HIGH);
54  analogWrite(ENA, motorSpeed);
55  analogWrite(ENB, motorSpeed);
56}
57// STOP
58void stopRobot() {
59  digitalWrite(IN1, LOW);
60  digitalWrite(IN2, LOW);
61  digitalWrite(IN3, LOW);
62  digitalWrite(IN4, LOW);
63  analogWrite(ENA, 0);
64  analogWrite(ENB, 0);
65}
66// TURN LEFT
67void turnLeft() {
68  digitalWrite(IN1, LOW);
69  digitalWrite(IN2, HIGH);
70  digitalWrite(IN3, HIGH);
71  digitalWrite(IN4, LOW);
72  analogWrite(ENA, motorSpeed);
73  analogWrite(ENB, motorSpeed);
74}
75// TURN RIGHT
76void turnRight() {
77  digitalWrite(IN1, HIGH);
78  digitalWrite(IN2, LOW);
79  digitalWrite(IN3, LOW);
80  digitalWrite(IN4, HIGH);
81  analogWrite(ENA, motorSpeed);
82  analogWrite(ENB, motorSpeed);
83}
84// AUTOMATIC OBSTACLE AVOIDANCE
85void automaticMode() {
86  int frontDistance = getDistance();
87  Serial.print("Front: ");
88  Serial.print(frontDistance);
89  Serial.println(" cm");
90  // Send distance to MIT App
91  BT.print("DIST:");
92  BT.println(frontDistance);
93  // ---------------- CLEAR PATH ----------------
94  if (frontDistance > safeDistance) {
95    moveForward();
96  }
97  // ---------------- OBSTACLE ----------------
98  else {
99    stopRobot();
100    delay(300);
101    // Move backward slightly
102    moveBackward();
103    delay(250);
104    stopRobot();
105    delay(300);
106    // ---------------- TURN LEFT ----------------
107    Serial.println("Obstacle detected");
108    Serial.println("Turning LEFT");
109    turnLeft();
110    delay(600);
111    stopRobot();
112    delay(200);
113  }
114}
115// MANUAL CONTROL
116void manualMode() {
117  switch (command) {
118    case 'F':
119      moveForward();
120      break;
121    case 'B':
122      moveBackward();
123      break;
124    case 'L':
125      turnLeft();
126      break;
127    case 'R':
128      turnRight();
129      break;
130    case 'S':
131      stopRobot();
132      break;
133  }
134}
135// SETUP
136void setup() {
137  // Ultrasonic
138  pinMode(TRIG_PIN, OUTPUT);
139  pinMode(ECHO_PIN, INPUT);
140  // L298N
141  pinMode(ENA, OUTPUT);
142  pinMode(IN1, OUTPUT);
143  pinMode(IN2, OUTPUT);
144  pinMode(IN3, OUTPUT);
145  pinMode(IN4, OUTPUT);
146  pinMode(ENB, OUTPUT);
147  // Serial Monitor
148  Serial.begin(9600);
149  // Bluetooth
150  BT.begin(9600);
151  stopRobot();
152  // ---------------- BLUETOOTH INFORMATION ----------------
153  Serial.println("================================");
154  Serial.println("   OBSTACLE AVOIDANCE ROVER");
155  Serial.println("================================");
156  Serial.print("Bluetooth Name: ");
157  Serial.println(bluetoothName);
158  Serial.println("Bluetooth Baud Rate: 9600");
159  Serial.println("Bluetooth TX -> Arduino D2");
160  Serial.println("Bluetooth RX -> Arduino D12");
161  Serial.println("================================");
162  BT.println("ROVER_READY");
163}
164// LOOP
165void loop() {
166  // Check Bluetooth command
167  if (BT.available()) {
168    char received = BT.read();
169    Serial.print("Received: ");
170    Serial.println(received);
171    // ---------------- AUTO ----------------
172    if (received == 'A') {
173      mode = 'A';
174      stopRobot();
175      BT.println("AUTO_MODE");
176      Serial.println("AUTO MODE");
177    }
178    // ---------------- MANUAL ----------------
179    else if (received == 'M') {
180      mode = 'M';
181      stopRobot();
182      BT.println("MANUAL_MODE");
183      Serial.println("MANUAL MODE");
184    }
185    // ---------------- MANUAL COMMANDS ----------------
186    else if (mode == 'M') {
187      command = received;
188    }
189    // ---------------- STOP ----------------
190    if (received == 'S') {
191      stopRobot();
192      command = 'S';
193    }
194  }
195  // ---------------- RUN MODE ----------------
196  if (mode == 'A') {
197
198    automaticMode();
199  }
200  else {
201
202    manualMode();
203  }
204  delay(50);
205}