DEV Community

Cover image for SmartRobot FollowLine & IoT
Jorge Martin
Jorge Martin

Posted on • Edited on

SmartRobot FollowLine & IoT

#arduino #cpp #programming

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>
Enter fullscreen mode Exit fullscreen mode
  • FastLED: facilitates efficient control of addressable LED, enabling advanced lighting effects and color management. 
#include "FastLED.h"
Enter fullscreen mode Exit fullscreen mode
  • ESP32: Necessary in order to establish a Wi-Fi connection with the ESP32 CAM. 
#include "WiFi.h"
#include "WiFiClient.h"
Enter fullscreen mode Exit fullscreen mode
  • 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"
Enter fullscreen mode Exit fullscreen mode

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)
Enter fullscreen mode Exit fullscreen mode

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
}
Enter fullscreen mode Exit fullscreen mode
  • 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
  }
}
Enter fullscreen mode Exit fullscreen mode
  • 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);
Enter fullscreen mode Exit fullscreen mode


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;
   }
 }
Enter fullscreen mode Exit fullscreen mode


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;
}
Enter fullscreen mode Exit fullscreen mode
  • 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
}
Enter fullscreen mode Exit fullscreen mode
  • 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
  }
}
Enter fullscreen mode Exit fullscreen mode
  • 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();
{
Enter fullscreen mode Exit fullscreen mode

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");

  // ...
}
Enter fullscreen mode Exit fullscreen mode

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.


Message of Arduino to ESP32:

Serial.print(3);
Serial.println(ultrasonic_distance);
Enter fullscreen mode Exit fullscreen mode


Processing of the 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);
}
Enter fullscreen mode Exit fullscreen mode

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
}
Enter fullscreen mode Exit fullscreen mode
  • 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
Enter fullscreen mode Exit fullscreen mode


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);
    }
  }
}
Enter fullscreen mode Exit fullscreen mode


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 = "";
}
Enter fullscreen mode Exit fullscreen mode


JSON message received on the server:

Image description

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)

Top comments (0)

Read next

mayank_mohapatra profile image

Creating a Multilingual Expense Tracker with Tolgee

Mayank Mohapatra -

karenpayneoregon profile image

C# Search by multiple strings

Karen Payne -

perisicnikola37 profile image

JavaScript SECRETS: Become a coding wizard!

Nikola Perišić -

arsenikavalchuk profile image

Manual Memory Management and Garbage Collection in Kotlin Multiplatform Native Shared Libraries

Arseni Kavalchuk -