Update Line Follower Code authored by Emílio Manuel Dias Mateus's avatar Emílio Manuel Dias Mateus
````#include:<Arduino.h>
#include <Wire.h>
#include <VL53L0X.h>
\`\`\`#include \<Arduino.h\>
#include \<Wire.h\>
#include \<VL53L0X.h\>
// Line Sensor Pins
const int PIN_IR_1 = 2;
const int PIN_IR_2 = 3;
const int PIN_IR_3 = 4;
......@@ -12,22 +10,16 @@ const int PIN_IR_4 = 5;
const int PIN_IR_5 = 6;
// Motor Sensor Pins
const int PIN_MOTOR_L_IN1 = 10;
const int PIN_MOTOR_L_IN2 = 11;
const int PIN_MOTOR_R_IN1 = 13;
const int PIN_MOTOR_R_IN2 = 12;
// ToF Sensor Pins
const int PIN_SCL = 17;
const int PIN_SDA = 16;
// Velocity Settings
const int VEL_NORMAL = 75;
const int VEL_SEARCH = 62;
const int VEL_SLOW = 40;
......@@ -35,33 +27,25 @@ const int VEL_FAST = 144;
const int VEL_REVERSE = -54;
// Time Settings for Obstacle Avoidance
const int TIME_LEFT = 700;
const int TIME_FORWARD = 1700;
const int TIME_RIGHT = 1500;
// Maximum Distance to Detect Obstacle
const int MAX_DISTANCE = 160;
// Objects and States
VL53L0X sensor;
bool robot_running = false;
int last_direction = 0;
// Function to Control Motor Speed and by PWM Values
void controlMotor(int pinIn1, int pinIn2, int pwm_val) {
pwm_val = constrain(pwm_val, -255, 255);
if (pwm_val > 0) {
if (pwm_val \> 0) {
analogWrite(pinIn1, pwm_val);
analogWrite(pinIn2, 0);
} else if (pwm_val < 0) {
} else if (pwm_val \< 0) {
analogWrite(pinIn1, 0);
analogWrite(pinIn2, -pwm_val);
} else {
......@@ -71,16 +55,12 @@ void controlMotor(int pinIn1, int pinIn2, int pwm_val) {
}
// Function to Stop the Robot Motors
void stopRobot() {
controlMotor(PIN_MOTOR_L_IN1, PIN_MOTOR_L_IN2, 0);
controlMotor(PIN_MOTOR_R_IN1, PIN_MOTOR_R_IN2, 0);
}
// Function to Avoid Obstacle Detected by ToF Sensor
void avoidObstacle() {
stopRobot();
delay(250);
......@@ -117,8 +97,6 @@ void avoidObstacle() {
}
// Robot Setup
void setup() {
Serial.begin(115200);
......@@ -150,8 +128,6 @@ void setup() {
}
// Loop Function
void loop() {
// Toggle Robot State with BOOTSEL Button
if (BOOTSEL) {
......@@ -165,7 +141,7 @@ void loop() {
// Obstacle Detection and Avoidance
uint16_t distance = sensor.readRangeContinuousMillimeters();
if (distance > 0 && distance < MAX_DISTANCE) avoidObstacle();
if (distance \> 0 && distance \< MAX_DISTANCE) avoidObstacle();
// Line Following Logic
// Read Line Sensors Values
......@@ -224,5 +200,4 @@ void loop() {
// Set Motor Speeds
controlMotor(PIN_MOTOR_L_IN1, PIN_MOTOR_L_IN2, motorL);
controlMotor(PIN_MOTOR_R_IN1, PIN_MOTOR_R_IN2, motorR);
}```
````
\ No newline at end of file
}\`\`\`
\ No newline at end of file