How to make a *Obstacle Avoiding* Car with LiDAR Sensor

1// LiDAR based Autonomous car || Obstacle Avoiding
2// Created By roboattic Lab
3// Contact me here https://www.instagram.com/roboattic_lab/
4
5#include <AFMotor.h>
6#include <Adafruit_VL53L0X.h>
7#include <Wire.h>
8
9#define MAX_DISTANCE 200
10#define MAX_SPEED 190
11#define MAX_SPEED_OFFSET 20
12
13AF_DCMotor motor1(1, MOTOR12_1KHZ); 
14AF_DCMotor motor2(2, MOTOR12_1KHZ);
15AF_DCMotor motor3(3, MOTOR34_1KHZ);
16AF_DCMotor motor4(4, MOTOR34_1KHZ);
17
18Adafruit_VL53L0X lox = Adafruit_VL53L0X();
19
20boolean goesForward = false;
21int distance = 100;
22int speedSet = 0;
23
24void setup() {
25
26  Serial.begin(115200);
27  delay(10);
28  Serial.println("VL53L0X test");
29
30  // Initialize I2C
31  Wire.begin();
32
33  // Add this to setup() before lox.begin() to scan for devices
34  Serial.println("Scanning I2C bus...");
35  for (byte i = 8; i < 120; i++) {
36    Wire.beginTransmission(i);
37    if (Wire.endTransmission() == 0) {
38      Serial.print("Found device at address: 0x");
39      Serial.println(i, HEX);
40    }
41  }
42
43  // Try to initialize the VL53L0X (only call begin() once)
44  if (!lox.begin()) {
45    Serial.println("Failed to find VL53L0X sensor. Check wiring!");
46    while (1)
47      ;  // Stop execution if sensor not found
48  }
49  Serial.println("VL53L0X initialized successfully.");
50}
51
52void loop() {
53
54  VL53L0X_RangingMeasurementData_t measure;
55
56  lox.rangingTest(&measure, false);
57
58  if (measure.RangeStatus == 4) {
59    Serial.println("Out of range");
60  } else {
61    distance = measure.RangeMilliMeter / 10;
62  }
63
64  // Print the distance to the serial monitor (for debugging)
65  Serial.print("Distance: ");
66  Serial.print(distance);
67  Serial.println(" cm");
68
69
70  int distanceR = 0;
71  int distanceL = 0;
72  delay(40);
73
74  if (distance <= 15) {
75    moveStop();
76    delay(100);
77    moveBackward();
78    delay(300);
79    moveStop();
80    delay(200);
81    distanceR = lookRight();
82    delay(200);
83    distanceL = lookLeft();
84    delay(200);
85
86    if (distanceR >= distanceL) {
87      turnRight();
88      moveStop();
89    } else {
90      turnLeft();
91      moveStop();
92    }
93  } else {
94    moveForward();
95  }
96}
97
98int lookRight() {
99  rotateRight();
100  delay(500);
101  VL53L0X_RangingMeasurementData_t measure;
102
103  lox.rangingTest(&measure, false);
104
105  if (measure.RangeStatus == 4) {
106    Serial.println("Out of range");
107  } else {
108    distance = measure.RangeMilliMeter / 10;
109  }
110  delay(500);
111  rotateLeft();
112  return distance;
113  delay(100);
114}
115
116int lookLeft() {
117  rotateLeft();
118  delay(500);
119  VL53L0X_RangingMeasurementData_t measure;
120
121  lox.rangingTest(&measure, false);
122
123  if (measure.RangeStatus == 4) {
124    Serial.println("Out of range");
125  } else {
126    distance = measure.RangeMilliMeter / 10;
127  }
128  delay(500);
129  rotateRight();
130  return distance;
131  delay(100);
132}
133
134
135void moveStop() {
136  motor1.run(RELEASE);
137  motor2.run(RELEASE);
138  motor3.run(RELEASE);
139  motor4.run(RELEASE);
140}
141
142void moveForward() {
143
144  if (!goesForward) {
145    goesForward = true;
146    motor1.run(FORWARD);
147    motor2.run(FORWARD);
148    motor3.run(FORWARD);
149    motor4.run(FORWARD);
150    for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2)  // slowly bring the speed up to avoid loading down the batteries too quickly
151    {
152      motor1.setSpeed(speedSet);
153      motor2.setSpeed(speedSet);
154      motor3.setSpeed(speedSet);
155      motor4.setSpeed(speedSet);
156      delay(5);
157    }
158  }
159}
160
161void moveBackward() {
162  goesForward = false;
163  motor1.run(BACKWARD);
164  motor2.run(BACKWARD);
165  motor3.run(BACKWARD);
166  motor4.run(BACKWARD);
167  for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2)  // slowly bring the speed up to avoid loading down the batteries too quickly
168  {
169    motor1.setSpeed(speedSet);
170    motor2.setSpeed(speedSet);
171    motor3.setSpeed(speedSet);
172    motor4.setSpeed(speedSet);
173    delay(5);
174  }
175}
176
177void turnRight() {
178  motor1.run(FORWARD);
179  motor2.run(FORWARD);
180  motor3.run(BACKWARD);
181  motor4.run(BACKWARD);
182  delay(500);
183  motor1.run(FORWARD);
184  motor2.run(FORWARD);
185  motor3.run(FORWARD);
186  motor4.run(FORWARD);
187}
188
189void turnLeft() {
190  motor1.run(BACKWARD);
191  motor2.run(BACKWARD);
192  motor3.run(FORWARD);
193  motor4.run(FORWARD);
194  delay(500);
195  motor1.run(FORWARD);
196  motor2.run(FORWARD);
197  motor3.run(FORWARD);
198  motor4.run(FORWARD);
199}
200
201
202void rotateLeft() {
203  motor2.run(FORWARD);
204  motor1.run(FORWARD);
205  motor4.run(BACKWARD);
206  motor3.run(BACKWARD);
207}
208void rotateRight() {
209  motor2.run(BACKWARD);
210  motor1.run(BACKWARD);
211  motor4.run(FORWARD);
212  motor3.run(FORWARD);
213}