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