This content originally appeared on DEV Community and was authored by Jorge Martin
This is the blog of the P4-SETR, where we programmed a follow line car using the Smart Robot Car V4.0 from Elegoo and Arduino IDE from arduino.
Coding
Libraries
During this proyect we need to use multiple libraries.
- FreeRTOS: enables the simultaneous execution of tasks in embedded projects, facilitating resource management and real-time control.
#include <Arduino_FreeRTOS.h>
- FastLED: facilitates efficient control of addressable LED, enabling advanced lighting effects and color management.
#include "FastLED.h"
- ESP32: Necessary in order to establish a Wi-Fi connection with the ESP32 CAM.
#include "WiFi.h"
#include "WiFiClient.h"
- MQTT: Facilitates MQTT communication by simplifying the process of connecting devices to MQTT brokers in IoT applications.
#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"
FreeRTOS
First of all we need to declare the task on the setup() using xTaskCreate following the structure:
xTaskCreate(FUNCTION, "task name", [STACK_SIZE], TASK_PARAMETERS, [TASK_PRIORITY], TASK_HANDLE)
In our case we have needed to implement 2 task. In order to ensure smoother vehicle movement and prevent potential blockages, we have assigned a higher priority to the infrared task compared to the ultrasound task.
-
Infrared task (follow line implementation)
- Setup(): on the setup function you must declare the callback_infrared function as a task.
void setup()
{
Serial.begin(9600);
// Infrared task configuration
xTaskCreate(callback_infrared, "Infrarrojo", 100, NULL, 1, NULL);
// Rest of the follow line specifications
}
- Task: in the function itself we must specify the time interval in which it should be executed at least once. Also, it is a periodic task, we need to use a while(1) loop.
static void callback_infrared(void* pvParameters)
{
while(1)
{
// Time in which the task must be executed
vTaskDelay(70 / portTICK_PERIOD_MS);
// Rest of the follow line implementation
}
}
- Follow-Line implementation: we have decided to use a switch case machine and take advantage of the fact that the sensor readings are analog to provide a more fluid and efficient movement.
First we adjust the engines to go forward and we store the sensor readings
// Adjust the engines to make the robot advance
digitalWrite(PIN_Motor_AIN_1, HIGH);
digitalWrite(PIN_Motor_BIN_1, HIGH);
// Read the infrered sensors
sensors[0] = analogRead(PIN_ITR20001_LEFT);
sensors[1] = analogRead(PIN_ITR20001_MIDDLE);
sensors[2] = analogRead(PIN_ITR20001_RIGHT);
Now, depending on the readings of the sensors we change the state of the machine
// Depending on the values of the infrared sensors gives the STATE variable a value or an other
if (sensors[1] > 800 && sensors[0] < 400 && sensors[2] < 400)
{
STATE = 1;
}
else if (sensors[0] > 400)
{
// Saves the sensors values to compare them once the line is lost.
prev_r_sensor = sensors[0];
prev_l_sensor = sensors[2];
STATE = 2;
}
else if (sensors[2] > 400)
{
// Saves the sensors values to compare them once the line is lost.
prev_r_sensor = sensors[0];
prev_l_sensor = sensors[2];
STATE = 3;
}
else if (sensors[0] < 400 && sensors[1] < 400 && sensors[2] < 400)
{
// Compares the latest values of the extremes to turn in the correct direction
if (prev_r_sensor > prev_l_sensor)
{
STATE = 4;
}
else if (prev_r_sensor < prev_l_sensor)
{
STATE = 5;
}
}
Depending on the state, we go forward, turn, or recover the line
// Control switch
switch (STATE)
{
case 1: // Forward
r_speed = 205 * 0.9;
l_speed = 205 * 0.9;
advance(r_speed, l_speed);
break;
case 2: // Turn left
r_speed = 150 * 0.9;
l_speed = 70 * 0.9;
advance(r_speed, l_speed);
break;
case 3: // Turn right
r_speed = 70 * 0.9;
l_speed = 150 * 0.9;
advance(r_speed, l_speed);
break;
case 4: // Recovers the line once it is lost on the right side.
recover_r();
break;
case 5: // Recovers the line once it is lost on the left side.
recover_i();
break;
}
-
Ultrasonic sensor
- Setup(): on the setup function you must declare the callback_check_distance function as a task.
void setup()
{
Serial.begin(9600);
// Ultrasonic task configuration
xTaskCreate(callback_check_distance, "Ultrasonido", 100, NULL, 3, NULL);
// Rest of the follow line implementation
}
- Task: as the callback_infrared task, in the function itself we must specify the time interval in which it should be executed at least once. Also, s it is a periodic task, we need to use a while(1) loop.
static void callback_check_distance(void* pvParameters)
{
while(1)
{
// Time in which the task must be executed
vTaskDelay(90 / portTICK_PERIOD_MS);
// Ultrasonic function implementation
}
}
- Ultrasonic function implementation: this function checks the distance with the object it has in front, if the distance is < 8cm it make the robot stop.
// Send a 10us pulse
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
// Calculate distance in cm
ultrasonic_time = pulseIn(ECHO_PIN, HIGH);
ultrasonic_distance = ultrasonic_time / 59;
// Detected object
if (ultrasonic_distance < 8)
{
Serial.println(ultrasonic_distance);
stop();
{
Communications
Before the Line Following behavior comes into action, it is essential to establish various forms of communication.
- Serial communication: It is necessary to establish serial communication between the ESP32 and Arduino boards. n our case, this communication is bidirectional; initially, the ESP32 sends a message to the Arduino board to establish the connection, and subsequently, the Arduino board sends corresponding actions to the ESP32. First, we need to configure the pins from which the ESP32 will read messages and establish the initial connection:
// Serial communication pins
#define RXD2 33
#define TXD2 4
void setup()
{
Serial.begin(9600);
// Serial communication
Serial2.begin(9600, SERIAL_8N1, RXD2, TXD2);
Serial2.print("{ 'test': " + String(millis()) + " }");
Serial.print("Messase sent! to Arduino");
// ...
}
Once the connection is established, the Arduino board sends numbers corresponding to each action along with their parameters. When the ESP32 receives these data, it determines which message to send to the MQTT server.
Mensaje de Arduino a ESP32:
Serial.print(3);
Serial.println(ultrasonic_distance);
Procesamiento de la ESP32:
if (Serial2.available())
{
char c = Serial2.read();
sendBuff += c;
// Process commands received from Arduino
}
else if (c == '3')
{
String incomingString = Serial2.readStringUntil('\n');
int distance = incomingString.toInt();
String obstaclePayload = "\n{\n\t\"team_name\": \"LiaDitto\",\n\t\"id\": \"6\",\n\t\"action\": \"OBSTACLE_DETECTED\",\n\t\"distance\": " + String(distance) + "\n}";
sendMQTTMessage(obstaclePayload);
}
For the communication to function, it is essential that the S1 switch on the expansion board be in the 'CAM' position.
- Wi-Fi connection: Once the serial communication has been established, the next step is to connect the device to the Wi-Fi network to enable communication with an MQTT server. The prior configuration of the network, including the network name and other details, is essential to ensure a successful connection.
void initWiFi()
{
WiFi.disconnect(true);
WiFi.begin(ssid, WPA2_AUTH_PEAP, EAP_IDENTITY, EAP_USERNAME, EAP_PASSWORD);
// Wait for WiFi connection
while (WiFi.status() != WL_CONNECTED)
{
delay(500);
}
MQTT_connect(); // Connect to MQTT broker after successful WiFi connection
}
- IoT communication: before sending messages to the MQTT server, it is crucial to consider the specific format and topic path. In this case, the messages are sent to a specific topic: /SETR/2023/$TEAM_ID/. Additionally, the server address and the MQTT server port must be taken into account:
Server: 193.147.53.2 (garceta.tsc.urjc.es)
Port: 21883
Connection to the server:
void MQTT_connect()
{
int8_t ret;
// Stop if already connected
if (mqtt.connected()
{
return;
}
uint8_t retries = 3;
while ((ret = mqtt.connect()) != 0) // connect will return 0 for connected
{
mqtt.disconnect();
delay(5000);
retries--;
if (retries == 0)
{
// If unable to connect after retries, halt execution
while (1);
}
}
}
To send messages, it is necessary for them to follow a predefined JSON format:
void sendMQTTMessage(const String& payload)
{
char str[payload.length() + 1];
payload.toCharArray(str, sizeof(str));
// Publish MQTT message
while (!photocell.publish(str))
{
}
sendBuff = "";
}
JSON message received on the server:
Functionality
Test (19/12/23)
This videos were a single take but recorded with two different devices, there might be a slight desynchronization between them.
(In case the video doesn´t play try this link: https://youtu.be/GdN1Jwjtsg8)
Final Test (21/12/23)
(In case the video doesn´t play try this link: https://youtu.be/ipnxLQI106w)
This content originally appeared on DEV Community and was authored by Jorge Martin
Jorge Martin | Sciencx (2024-09-11T12:13:10+00:00) SmartRobot FollowLine & IoT. Retrieved from https://www.scien.cx/2024/09/11/smartrobot-followline-iot/
Please log in to upload a file.
There are no updates yet.
Click the Upload button above to add an update.