DEV Community: Jorge Martin

Blog Robotica de Servicios

Jorge Martin — Fri, 26 Sep 2025 09:34:43 +0000

Index

P1: Localized Vacuum Cleaner
P2: Rescue People
P3: Autoparkin
P4: Amazon Warehouse
P5: Laser Mapping
P6: Marker Visual Loc

P1: Localized Vacuum Cleaner

Objective

The objective was to program a high-end robotic vacuum cleaner to clean a house efficiently. To achieve this, a BSA algorithm will be used. Additionally, taking advantage of the fact that it is a high-end vacuum cleaner, a self-localization algorithm will also be employed.

Procedure

1. Coords conversion

The robot is simulated in Gazebo’s 3D environment, and I need to convert its 3D world coordinates into 2D pixel coordinates. For this purpose, the following formula is applied:

However, since the goal is a 2D transformation, the z-coordinate can be discarded, and as the angular component remains unchanged, the formula can be reduced to:

To determine the scale, I recorded several pixel positions and their corresponding simulator coordinates. This was done roughly by eye, so multiple points were used to reduce error. The calculated scale is 101.5, with the x-scale equal in magnitude but negative compared to the y-scale, reflecting the axis inversion between pixel and simulator coordinates.

Using the reference pixel position (0,0), which corresponds to world coordinates (5.6,−4.17), the translations were calculated as the differences between pixel and simulator coordinates, resulting in 𝑡x=−5.6 and 𝑡y=4.17.

Now that we have calculated everything, the transformation matrix can be expressed as follows:

2.Grid map

To divide the map into cells, the first step is to determine their size. Since the robot measures 35x35 pixels, the cells should be slightly larger without being excessive. Therefore, I decided to make each cell 37x37 pixels.

Next, the obstacles are expanded so that if the percentage of black pixels within a cell exceeds 12%, that cell is assigned a value of 0, corresponding to an obstacle. Otherwise, it is assigned a value of 2, representing a dirty cell.

# Cells values
CELL_COLORS = {
    0: 0,    # Obstacle: black
    1: 131,  # Clean: green
    2: 129,  # Dirty: orange
    3: 132,  # Return: blue
    4: 134,  # Critic: violet
    5: 130   # Plan: yellow
}

Each cell has a value between 0 and 5, depending on the type of cell it represents.

3.Return and critical points

Among the different types of cells, two are essential: critical points and return points.

Critical points are determined based on the immediate neighbors of a cell (N, S, E, W). If all of its neighboring cells are obstacles and/or already cleaned cells, the cell becomes a critical point.

Return points are checkpoints where the vacuum can return once it reaches a critical point. These are generated in all immediate neighboring cells (N, S, E, W) that are neither obstacles nor already cleaned cells.

4.Route planning

To plan the route, the BSA algorithm is used, where initially the entire map consists of obstacles and dirty cells. As the robot moves, cells are marked as cleaned, while critical points and return points are identified. The algorithm enables the robot to systematically traverse all accessible areas, avoiding obstacles and optimizing the path to cover the maximum space without unnecessary movements.

During the route planning phase, the algorithm evaluates the neighboring cells of the current position in a fixed order: North, East, South, and West. The first neighboring cell that is neither an obstacle nor already planned is selected as the next cell to visit.

When the robot reaches a critical point, it cleans it and selects the return point based on the smallest Manhattan distance.

for rc in return_points:
        dist = abs(rc[0] - r) + abs(rc[1] - c)

Once the return point is selected, the robot uses a Breadth-First Search (BFS) algorithm to determine the shortest path to that point. BFS explores the neighboring cells systematically, avoiding obstacles and already cleaned cells, ensuring that the robot reaches the return point efficiently while covering all accessible areas.

5.Movement

During the planning stage, the program stores in an array the positions of the centers of the cells in the order they should be visited. This ensures that the robot stays as centered as possible while moving through each cell, guaranteeing full cleaning coverage and reducing the risk of collisions with obstacles.

Once the robot has identified the next cell center to visit, it must decide which direction to move in. The robot can move in four possible directions — North, South, East, and West — and the decision is based on the difference between the robot’s current position and the position of the target cell center.

if (abs(diff_u) > abs(diff_v)):
        if (diff_u < 0):
            direction = "East"
        else:
            direction = "West"
    else:
        if (diff_v < 0):
            direction = "South"
        else:
            direction = "North"

To rotate the robot, the system uses the yaw angle. Knowing the ideal yaw value for each direction, the robot turns until it reaches the angle corresponding to the direction it needs to follow.

if direction == "North":
        target_yaw = -np.pi/2
    elif direction == "South":
        target_yaw = np.pi/2
    elif direction == "East":
        target_yaw = np.pi
    elif direction == "West":
        target_yaw = 0.0

Challenges

One of the main challenges I faced throughout the project was the coordinate transformations from 3D to 2D. I had to identify multiple reference points to minimize errors, determine the correct scale, and construct the transformation matrix accurately.
Another challenge was planning the return route from a critical point to a return point, as I had to implement a BFS algorithm to determine the optimal path.

Functionality

(In case the video doesn´t play try this link: https://youtu.be/wA_xdow-XGE)

P2: Rescue People

Objective

The objective of this exercise was to program the behavior of a drone that must go to an area where people are floating in the sea, recognize the faces of those people, and record their coordinates so that rescue services can save them.

Procedure

1. Coords coversion

The first step is to convert the GPS coordinates to local coordinates. To do this, I first transform the GPS data into UTM coordinates.

The next step is to interpret the UTM coordinates. After that, we obtain the global positions of both the base and the rescue area. Once this is done, we convert these coordinates into the local reference frame, assuming that the base is located at the point (0, 0) in the local system.

2. Camera FOV

The next fundamental step was to determine the drone camera’s FOV, since it is needed to calculate the spacing between the route points and, later, to determine the local coordinates of detected faces.

I determined the FOV by using the drone’s starting base as a reference and moving the drone a certain distance at a fixed height of 5 m until the base was no longer visible.

3 Generate route

Knowing the coordinates of the rescue area, I defined a search zone. Considering the area covered by the drone’s camera, I divided this zone into points for the drone to follow, ensuring complete coverage of the search area. To avoid gaps, I set the spacing between points to 0.8 times the drone’s camera range, reducing the risk of leaving any areas unscanned.

4. FSM Behaviour

4.1 Reach the rescue zone

Knowing the position of the rescue area, using a position-based control is the simplest way to move the drone to that location.

zone_coords = (40, -30, 5)
HAL.takeoff(5)
HAL.set_cmd_pos(zone_coords[0], zone_coords[1], zone_coords[2], 0)

4.2 Face detection

To detect people’s faces, I used the drone’s camera along with the Haar Cascade classifier. Since the faces appear relatively small due to the drone’s altitude, it was necessary to adjust the detection parameters. Specifically, I modified the minimum size and the number of neighbors.

faces = face_cascade.detectMultiScale(
                gray, scaleFactor=1.1, minNeighbors=4,
                minSize=(20, 20), maxSize=(100, 100)

To avoid capturing duplicate faces, I store the local coordinates of detected faces. If the distance between a new detection and an existing one is below a threshold, I assume it’s the same face and discard it to prevent duplicates.

for x, y in coords:
        distance = math.sqrt((x - x_new)**2 + (y - y_new)**2)
        if distance < coord_threshold:
            return True
    return False

4.3 Select next point

Since the route points are stored in an array, moving to the next point is straightforward: simply pop the next point from the array and use position-based control to reach it.

if scan_points:
                current_scan_point = scan_points.pop(0)
                HAL.set_cmd_pos(current_scan_point[0],
                                current_scan_point[1],
                                5,
                                0)

4.4 Return to base

To return to the base, I again use position-based control, since the base’s position is known.

HAL.set_cmd_pos(0, 0, 1.5, 0)
HAL.land()

Challenges

The only difficulty I faced was the multiple detection of the same face due to the detectMultiScale parameters. I solved it by applying a coordinate filter to remove duplicate detections.

Functionality

(In case the video doesn´t play try this link: https://youtu.be/-RhBi4Nvp_I)

P3: Autoparking

Objective

The main objective of the exercise was to design the behavior of an autonomous car so that it could correctly park in an available space.

Procedure

To design the robot’s behavior, I used a finite state machine (FSM).

1. INIT_ALIGN state

To align the car, I used the laser sensors along with NumPy’s SVD function.. Valid measurements from the right-side laser are filtered, the coordinates are centered, and SVD is applied to determine the direction of the wall. Using the resulting angle, the car adjusts its orientation until properly aligned before transitioning to the CRUISE state.

2. CRUISE state

The Cruise state moves the car forward, keeping it aligned with the street using the yaw angle calculated in the previous state as a reference. During this process, the vehicle drives along the street while using the side laser sensor to detect a space large enough to park in

3. PREPARE_TO_PARK state

In this state, the car prepares to park by positioning itself advantageously.

3.1 approach substate

In this substate, the car moves forward while turning right to approach the parking area. Once it reaches a certain angle or detects an obstacle in the front, it transitions to the next substate.

3.2 align substate

In this substate, the car realigns itself using the initial yaw angle calculated in the INIT_ALIGN state as a reference. Once aligned, it transitions to the next state to begin the parking process.

4. REVERSE_PARK

This state handles reversing the car into a parking space, controlling its orientation and position using the front and rear laser sensors.

4.1 start_entering substate

In this phase, the car begins entering the parking space in reverse while turning. The yaw error relative to the initial yaw is calculated to ensure the vehicle does not deviate excessively. The car stops and transitions to the next phase if the rear gets too close to an obstacle or if the yaw exceeds a predefined threshold.

4.2 maneuver_back substate

In this phase, the car continues reversing while slightly adjusting its orientation to align correctly within the parking space. The maneuver stops once the rear reaches a safe distance and the yaw is properly aligned.

4.3 maneuver_front substate

In the final phase, the car moves forward slightly to adjust its position and complete the parking maneuver. The distance traveled and the proximity of front obstacles are monitored. Once the car reaches the correct position and orientation, all motion stops, completing the parking process.

Challenges

The only challenge I faced was aligning the car at the start using SVD, since I had never used it before.

Functionality

Between 2 cars

(In case the video doesn´t play try this link: https://youtu.be/p3eFg56Gpjs)

Open Spot

(In case the video doesn´t play try this link: https://youtu.be/E89zxtju4Fs)

P4: Amazon Warehouse

Objective

The objective of this assignment was to program the behavior of a robot that needed to navigate a warehouse to pick up shelves and move them to other locations, using the OMPL library and planning with the robot’s geometric constraints.

Procedure

1. Coordinate conversion

The first step of this assignment was to perform a coordinate conversion from map to world and vice versa. For this, the map dimensions (279×415 px) and the warehouse dimensions (20.62 meters long and 13.6 meters wide) were used. From these, we can convert coordinates from the map to the world and from the world to the map.

def pixel_to_world(px, py):
    world_x = (center_y - py) / (map_height / world_dim[1])
    world_y = (center_x - px) / (map_weight / world_dim[0])

    return world_x, world_y

def world_to_pixel(world_x, world_y):
    px = int(center_x - world_y * (map_weight / world_dim[0]))
    py = int(center_y - world_x * (map_height / world_dim[1]))

    return px, py

2. Obstacle initialization

To initialize the obstacles, I simply traversed the map array and analyzed its values. Anything different from 127 (white, representing free space) was converted to world coordinates and stored in an array.

3. Path planning

The robot’s routes were planned using the OMPL library, taking into account the robot’s rectangular geometry.

def isStateValid(state):
    x, y, yaw = state.getX(), state.getY(), state.getYaw()
    width, height = robot_dim
    r = 0.15

    points = createGridPoints(width, height, 3)
    world_points = rotateAndTranslate(points, x, y, yaw)

    for px, py in world_points:
        if outsideWorld(px, py, world_dim):
            return False
        for ox, oy in obstacles:
            if distance(px, py, ox, oy) < r:
                return False
        ...
    ...
    return True

3.1 Holonomic robot

For planning with the holonomic robot, SE2StateSpace was used, providing a straightforward representation of the robot’s position and orientation, combined with the RRTConnect algorithm to generate efficient paths.

3.2 Ackermann robot

For the Ackermann robot, DubinsStateSpace was used, which allows modeling paths with minimum turning radius constraints, along with the RRTstar algorithm, which not only finds feasible routes but also gradually optimizes them to produce more efficient trajectories.

4.Movement

4.1 Holonomic robot

Programming the movement of the holonomic robot was relatively straightforward, as it can rotate in place. Essentially, the robot continuously adjusts its angular velocity to stay aligned with the angle of the next waypoint along the path while moving forward.

4.2 Ackermann robot

Programming the movement of the Ackermann robot was somewhat more complex, as it cannot rotate in place and must respect minimum turning radius constraints. The robot continuously adjusts its wheel angles to follow the planned path, ensuring that the turns are feasible according to its geometry and turning capabilities.

5.Robot return

To return, a new route must be planned, since the robot’s dimensions change when carrying a shelf, and it may not be able to follow the same path back. However, the robot’s movement mode remains the same.

Challenges

One of the problems I encountered was with coordinate conversion, since the map coordinates did not match those of the real world or the robot.
Another problem was navigating the Ackermann robot, as it is considerably more complex than navigating a holonomic robot.

Functionality

Holonomic

(In case the video doesn´t play try this link: https://youtu.be/XRnuJpdt9N8)

Ackermann

(In case the video doesn´t play try this link: https://youtu.be/874kHCtQc8o)

P5: Laser Mapping

Objective

The objective was to program the robot’s behavior to perform laser-based mapping of a warehouse.

Procedure

Movement

The robot’s movement behavior is based on a Finite State Machine (FSM), and its motion follows a serpentine pattern.

LIDAR sensing

The robot uses LIDAR to build a real-time probabilistic map, identifying free and occupied spaces. The continuously updated map allows autonomous movement, collision avoidance, and efficient exploration.

Probabilistic mapping

The robot uses probabilistic mapping with LIDAR to explore autonomously. Each map cell’s probability is updated with Bayes’ theorem, combining sensor measurements with prior information to handle uncertainty and improve accuracy.

P_prior: the prior probability that the cell is occupied.
P_measure: the probability of occupancy based on the LIDAR measurement.
P_posterior: the updated probability that the cell is occupied after combining the prior and the measurement using Bayes’ theorem.

Building the map

In the visualization of the probabilistic map, each cell is represented by a grayscale value:

black (0) for obstacles with high probability (>0.7)
white (255) for free space with low probability (<0.3)
gray (127) for unknown areas where the probability is between 0.3 and 0.7.

# Mask
occupied_mask = prob_map > 0.7
free_mask = prob_map < 0.3
unknown_mask = np.logical_and(prob_map >= 0.3, prob_map <= 0.7)

# Values
img[free_mask] = 255
img[occupied_mask] = 0
img[unknown_mask] = 127

Challenges

The main issue was that the robot would sometimes get stuck moving in an infinite closed loop. I solved this by making the robot move a random distance between 0.4 m and 2 m during the displacement phase, preventing it from becoming trapped in a loop.

Functionality

Small Warehouse

(In case the video doesn´t play try this link: https://youtu.be/pTc4MeFgteo)

Big Warehouse

(In case the video doesn´t play try this link: https://youtu.be/gTbYZlPK2oY)

Final results

Small Warehouse + getPose3d()

Small Warehouse + getOdom()

Small Warehouse + getOdom2()

Small Warehouse + getOdom3()

Big Warehouse after 100 min

P6: Marker Visual Loc

Objective

The aim was to develop the robot's behavior to enable self-localization on a map by relying on Apriltags when they were within view, and using odometry as a fallback when no tags were detected.

Procedure

Movement

For the robot's movement, I implemented a simple FSM in which the robot moves forward until it detects an obstacle, then turns a random angle between 45 and 60 degrees.

AprilTags Loading

One of the initial steps was to load the YAML file containing the actual positions of the tags, so that the robot could accurately localize itself in the future.

conf = yaml.safe_load(Path("/resources/exercises/marker_visual_loc/apriltags_poses.yaml").read_text())
tags = conf["tags"]

Self-Localization

By AprilTags

The robot detects AprilTags within its camera view to estimate its pose on the map. When a tag is visible and within a valid distance and angle, the robot calculates its position relative to the tag using the PnP algorithm. This relative pose is then transformed into global coordinates based on the tag’s known position, providing an accurate estimate of the robot’s location.

By Odom

When no AprilTags are visible, the robot relies on odometry to estimate its position. It calculates the displacement and rotation since the last visual estimate and updates its global pose accordingly.

This approach allows the robot to maintain an approximate position on the map until a tag becomes visible again, ensuring continuous localization even in the absence of visual references.

Challenges

Laser: one of the main issues was that int the API indicated the laser had a range of 0 to 180 degrees, when in fact it was 0 to 360 degrees. This caused the robot to behave erratically until I identified the underlying cause.

Functionality

(In case the video doesn´t play try this link: https://youtu.be/X4tzMItDPqo)

Monte Carlo Laser Localization

Jorge Martin — Sun, 22 Dec 2024 17:35:39 +0000

This is the blog of the P5-RM, where I programmed a Monte Carlo Laser Localization algorithm.

Objetive

The objective of this project is to program a localization algorithm based on the Monte Carlo algorithm. The developed algorithm must estimate the robot's position on the map. To achieve this, the robot is equipped with a laser sensor and has access to the environment map.

Introduction to Monte Carlo Laser Localization

Monte Carlo Laser Localization (MCLL) is a probabilistic algorithm used in robotics for determining a robot's position and orientation within a given environment. By utilizing laser range finders, MCLL samples a set of possible positions (particles) and updates their likelihood based on sensor data. This method allows for accurate and efficient localization even in complex and dynamic environments, making it a popular choice for autonomous navigation systems.

How does it works?

A number of particles are randomly generated within the map boundaries and in free cells, that is, cells with a value of 255.
The program calculates the robot's relative position using the average x, y, and yaw of all the particles.
Using ray tracing and the robot's actual laser, the differences between the real measurement and each of the measurements made by the particles are calculated.
Weights inversely proportional to the difference between the simulated and real measurements (less error, more weight) are assigned to each particle nd then normalizing all of these, considering whether they are in a valid position on the map.
The program performs resampling of the particles based on their weights, selecting those with higher weights (less error) with greater probability to focus the estimation on the most accurate robot positions.
The particles move based on the robot's linear and angular velocity and the time that has passed since the previous iteration.
Repeats from step 2

Parameters

After several tests, the parameters I have left in the code are:

# Map Data
MAP_FREE = 255
MAP_OCC = 0
MAP_URL = '/resources/exercises/montecarlo_laser_loc/images/mapgrannyannie.png'

# Raytracing and Laser Data
MAX_LASER_DISTANCE = 10
LASER_NUM_BEAMS = 3
RAYTRACING_SKIP_STEPS = 1

# Noise parameters
PROPAGATION_XY_NOISE_STD = 0.005
PROPAGATION_ANGLE_NOISE_STD = 0.001

# Constant robot velocities
LINEAR_VEL = 0.2
ANGULAR_VEL = 0.2

As for the number of particles used, I recommend using between 150 and 1500 particles. Fewer particles would make it quite inaccurate, and more particles would slow down the process due to the increased number of calculations.

Difficulties

It failed a lot because at first it considered particles that were inside obstacles and/or outside the map. I fixed this by adding a couple of functions that automatically assign a weight of 0 to these particles.
Finding the appropriate values is important, as performance and accuracy would decrease or increase significantly depending on the number of particles, beams, etc.
I think it can still be fine-tuned more, but I'm not sure why it doesn't converge exactly. It might be due to the weight assignment method.

Functionality

50 PARTICLES

(In case the video doesn´t play try this link: https://youtu.be/NxI02HzUS5k)

150 PARTICLES

(In case the video doesn´t play try this link: https://youtu.be/LUVkUov7j7I)

300 PARTICLES

(In case the video doesn´t play try this link: https://youtu.be/K9s6kuokz18)

750 PARTICLES

(In case the video doesn´t play try this link: https://youtu.be/tKw9rA3ff_Q)

1500 PARTICLES

(In case the video doesn´t play try this link: https://youtu.be/mbrDgXKhjvM)

Global Navigation + Gradient Path Planning

Jorge Martin — Mon, 25 Nov 2024 19:47:27 +0000

This is the blog of the P4-RM, where I programmed the behavior of a taxi based on Gradient Path Planning.

Objetive

The objective of this practice was to program the behavior of a taxi using gradient path planning, which generates a cost map and moves following the path of least cost until reaching a target.

Coding

Libraries

This library provides access to mathematical functions and constants.

import math

This library provides functions for working in domain of linear algebra, fourier transform, matrices and arrays.

import numpy as np

Implementation

Cost Map

The creation of the cost map consists of three distinct functions: one that creates the simple cost map, another that uses this same cost map to expand the obstacles and penalize the taxi for getting too close to them, and a third function that normalizes the costs.

MAP_URL = '/resources/exercises/global_navigation/images/cityLargenBin.png'
city_map = GUI.getMap(MAP_URL)

def normalize_cost(cost_grid, robot_map, target_map):
    ... 
    normalized_grid = np.full_like(cost_grid, np.inf)
    normalized_grid[finite_mask] = (cost_grid[finite_mask] / max_cost) * 255

    return normalized_grid

def show_cost(target_map, robot_map, city_map):
    cost_grid = np.full(city_map.shape, np.inf)
    cost_grid[target_map[0], target_map[1]] = 0
    queue = [(target_map[0], target_map[1])]

    ...

    # Normalize
    normalized_grid = normalize_cost(cost_grid, robot_map, target_map)
    return normalized_grid

def expand_obstacle(cost_grid, city_map, cushion_size=3, cushion_cost=255):
    ...

    return expanded_grid

Navigation

The car navigates based on the gradient, it obtains the cost map and analyzes its 8 neighboring cells. These neighbors can range from unreachable (not expanded or obstacles) to cells with varying costs. Among the latter, the taxi will move toward the cell with the lowest cost. This makes the navigation semi-reactive.

def navigate_to_target(cost_grid, robot_map, target_map):
    ...

    while robot_map != target_map:
        # Neighbors of the robot
        neighbors = [(robot_map[0] - 1, robot_map[1]), (robot_map[0] + 1, robot_map[1]),
                     (robot_map[0], robot_map[1] - 1), (robot_map[0], robot_map[1] + 1),
                     (robot_map[0] - 1, robot_map[1] - 1), (robot_map[0] + 1, robot_map[1] + 1),
                     (robot_map[0] - 1, robot_map[1] + 1), (robot_map[0] + 1, robot_map[1] - 1)]

        ...

        next_step = min(valid_neighbors, key=lambda x: cost_grid[x[0], x[1]])

        ...
        next_grid = GUI.gridToWorld((next_step[1], next_step[0]))

        while distance > 0.3:
            ...
            target_grid_x, target_grid_y = next_grid
            direction = math.atan2(distance_y, distance_x)
            angle_diff = math.atan2(math.sin(direction - robot_orientation), math.cos(direction - robot_orientation))

            linear_speed = 6 if abs(angle_diff) < 0.2 else 0
            angular_speed = 0 if abs(angle_diff) < 0.2 else angle_diff * 2

            HAL.setV(linear_speed)
            HAL.setW(angular_speed)
        ...

        robot_map = next_step

Difficulties

One of the main problems I encountered was during navigation. There were times when two adjacent cells had the same cost or were surrounded by higher-cost cells, which caused the robot to get stuck in a constant loop. I solved this by prohibiting the robot from returning to a previous cell.
Another problem I encountered was that when converting from world to grid, the x and y coordinates were inverted, as x referred to the columns and y to the rows.

Logbook

Day 1: Test the world and implement conversions from world coordinates to grid.
Day 2: Create a simple cost map.
Day 3: Enhance the cost map by expanding obstacles.
Day 4: Normalize the cost map and finalize its generation when it reaches the taxi.
Day 5: Implement navigation.

Functionality

Simple test

(In case the video doesn´t play try this link: https://youtu.be/wn9l0Gxg-0M)

Test long ride

I placed the first target in the bottom-left corner and the second one in the top-right corner to observe the taxi's behavior during a very long ride.

(In case the video doesn´t play try this link: https://youtu.be/45cWqGK-cH4)

Changing target in the middle of the ride

The taxi reaches the first target before changing to the next one.

(In case the video doesn´t play try this link: https://youtu.be/hGz6yQENSe0)

F1 Local Navigation + VFF

Jorge Martin — Sun, 27 Oct 2024 15:35:37 +0000

This is the blog of the P3-RM, where I programmed the behavior of a F1 with VFF.

Objective

The objective of the practice was to program the behavior of an F1 car using VFF (Vector Field Following) to follow targets placed on the map while avoiding obstacles on the road, in this case, other cars.

Coding

Libraries

This library provides access to mathematical functions and constants.

import math

Implementation

Handling of the targets

We obtain the target, but when the car is close and since the distance to the target is unlikely to be exactly 0, it changes the target when it is relatively close.

dx = target_global_pose.x - robot_pose.x
dy = target_global_pose.y - robot_pose.y
distance = math.sqrt(dx**2 + dy**2)

if distance < TARGET_THRESHOLD:
      target.setReached(True)
      target = GUI.getNextTarget()
      target_global_pose = target.getPose()

Attractive force

The attractive force is calculated using the current position of the car and the position of the next target, so that the greater the distance, the more force is applied.

distance = math.sqrt(x_rel**2 + y_rel**2)
magnitude = max(MIN_FORCE, min(distance, MAX_FORCE))

a_force_x = magnitude * (x_rel / distance)
a_force_y = magnitude * (y_rel / distance)

Repulsive force

To create the repulsive force, we will use a laser. Depending on the position and distance of the detected object, there is a priority order:

Objects VERY close to the car
Objects in front of the car
Objects on the sides

laser_data = HAL.getLaserData()
values = laser_data.values

for i in range(70, 110):
      # Here we store the min_distance

if min_distance >= 8.0:
      for i in range(len(values)):
      # Here we store the min_distance

if any(value < 0.7 for value in values):
      # Here we store the angle corresponding to that distance.

angle = math.radians(min_index - 90)

raw_repulsion_magnitude = -RF_BASE / max(min_distance, 0.1)
repulsion_magnitude = max(-MAX_FORCE, min(raw_repulsion_magnitude, 0))

r_force_x = repulsion_magnitude * math.cos(angle)
r_force_y = repulsion_magnitude * math.sin(angle)

Combined force

The total force that we will later use to derive the velocities is a weighted sum of the attractive and repulsive forces.

Ka = 2
Kr = 3

combined_force = [Ka * attractive_force[0] + Kr * repulsion_force[0], 
                  Ka * attractive_force[1] + Kr * repulsion_force[1]]

Transform the forces into velocities.

Once we have calculated the total force, we can derive the linear and angular velocities such that the linear velocity is the magnitude of the force and the angular velocity is calculated as atan(F_y / F_x).

linear_vel = math.sqrt(combined_force[0]**2 + combined_force[1]**2)
linear_vel = max(MIN_VEL, linear_vel)

angular_vel = math.atan2(combined_force[1], combined_force[0])

HAL.setV(linear_vel)
HAL.setW(angular_vel)

Difficulties

At one point, the repulsive force acted like an attractive force, and I had to remove several things and step back a bit to fix that issue.
Modify the constants Ka and Kr to optimize the movement.

Logbook

Day 1 (20/10/24): Implementation of tracking and target switching.
Day 2 (21/10/24): Implementation of repulsive force.
Day 3 (23/10/24): Fixing issues with the repulsive force.
Day 4 (25/10/24): Since I couldn't fix it, I’ll remove the velocity to focus on fixing the forces.
Day 5 (27/10/24): Implementation of velocities.

Functionality

F1 FollowLine + HSV filter + PID Controller

Jorge Martin — Fri, 11 Oct 2024 09:21:36 +0000

This is the blog of the P2-RM, where I programmed the behavior of a F1 Follow-Line car.

Objective

The objective of this project is to program the behavior of an F1 car, so that it can safely and quickly complete a lap around a circuit. To achieve this, I will use an HSV filter to detect the line and a PID controller.

Coding

Libraries

This library provides functions related to time management, such as getting the current time or measuring elapsed time.

import time

This library provides functions for working in domain of linear algebra, fourier transform, matrices and arrays.

import numpy as np

This library is used for image and video processing, offering functions for tasks like object detection, filtering, and transformations in computer vision.

import cv2

Implementation

HSV Filter

To create the HSV filter, we will need to generate two masks because the red color occupies two regions in the hue spectrum.

For each red value, a hue, saturation, and value are necessary.

lower_red_1 = np.array([0, 120, 70])
upper_red_1 = np.array([10, 255, 255])
lower_red_2 = np.array([170, 120, 70])
upper_red_2 = np.array([180, 255, 255])

Centroids

At the beginning, I decided to place 3 centroids at three different points within a specific area of the red line.

However, to make it simpler and more precise, I changed it to extract a single centroid from a specific area using image moments.

if moments['m00'] != 0:
       centroid_x = int(moments['m10'] / moments['m00'])

PID

To control the movement of the car, I have implemented a PID controller where:

P = Kp * error

integral += error * dt
I = Ki * integral

derivative = (error - previous_error) / dt 
D = Kd * derivative

In this case, dt is the time difference between the current measurement and the last recorded one.

For each map, it was necessary to tweak the values of the Kp, Ki, and Kd components to optimize the car's movement.

# Values for Simple
Kp = 0.011
Ki = 0.0015
Kd = 0.0004

Difficulties

The only major problem I've had has been with the PID controller, as even the slightest variation in any component caused the car to crash. It took quite some time to find the right values for each map.

Logbook

Day 1 (30/09/24): Implementation of the HSV filter (not tested on all maps).
Day 2 (2/10/24): Improvement of the HSV filter and analysis of the regions to be examined.
Day 3 (8/10/24): Change from 3 centroids in a line to 1 centroid in an area.
Day 4 (9/10/24): Implementation of the PID controller and adjustment of the components Kp, Ki and Kd.

Functionality

⚠️Warning⚠️: due to the constant change in the computer's performance, there are times when the times may decrease, increase, or even when the car may never complete the lap.

Simple: 31.46s

Simple Ackerman: 135s

Vacuum Cleaner

Jorge Martin — Tue, 17 Sep 2024 18:14:42 +0000

This is the blog of the P1-RM, where I programmed the behavior of a vacuum cleaner (Roomba).

Objective

The objective of this project was to program the behavior of a vacuum cleaner robot so that it could autonomously, reactively, and safely navigate the environment. The robot utilized sensors such as bumpers and a laser to detect obstacles and adjust its movements accordingly.

FSM

The functionality of the code resides in a finite state machine with 3 states:

Spiral: the robot starts by moving in a spiral until it reaches maximum speed or crashes, at which point it switches to turn.
Turn: the robot depending on the bumper hit, it turns left or right and a random time each time, when the time's up, it switches to forward.
Forward: the robot starts going forward, if the time's up it switches to spiral, while moving, if the robot hit something or the laser detects an obstacle in front of the robot, it switches to turn.

Coding

Libraries

This library provides functions to generate random numbers, select random elements from sequences, shuffle lists, and other functionalities related to randomness.

import random

This library provides functions related to time management, such as getting the current time or measuring elapsed time.

import time

Implementation

The most important function is the execute() function, as it will be running constantly. This function is also responsible for deciding which state to transition to, depending on the sensor readings.

vacuum = VacuumCleaner()

while True:
    vacuum.execute()

I implement a random turn and forward movement, I achieved this by adjusting their durations. To do so, I incorporated a timer that measures the time elapsed since the movement started.

self.turn_start_time = time.time()
self.turn_duration = random.uniform(0.5, 1.25)

self.elapsed_turn_time = time.time() - self.turn_start_time
if self.elapsed_turn_time >= self.turn_duration:
   self.end_turn = True

Difficulties

Random Movement: one of the main challenges was balancing the random time and distance factors. The robot needed to move in a seemingly unpredictable way while ensuring it covered most of the environment efficiently.

Logbook

Day 1 (16/09/24): I focused on implementing the skeleton of the finite state machine with the three states (Forward, Turn, Spiral).
Day 2 (17/09/24): I have implemented the turning function with random times and I also finished the spiral movement function.
Day 3 (18/09/24): Implementation of a timer in the forward function to cover a random distance each time. If it completes the time, it starts to perform the spiral; if it collides, it turns.
Day 4 (21/09/24): Program completed, implementation of the laser, and depending on which bumper it hits, it will turn one way or the other.
Day 5 (23/09/24): Final tests and minor adjustments to the speeds and times.

Functionality

Test of the robot starting in the samallest room [5min]

(In case the video doesn´t play try this link: https://youtu.be/vAxX1OhKjNM)

Cleaning Test [20min]

(In case the video doesn´t play try this link: https://youtu.be/dWYY3eG4c-U)

Cleaning Test [1h]

(In case the video doesn´t play try this link: https://youtu.be/VXfXEwkdKzQ)

Vending Machine Controller

Jorge Martin — Wed, 11 Sep 2024 12:13:36 +0000

This is the blog of the P3-SETR, where I have created a Vending Machine Controller using an Arduino Kit.

Schematic

Here I show you the schematic of the circuit.

Components

Component Name	Amount
Arduino Uno board	x1
Protoboard	x1
LEDs	x2
Button	x1
DHT11 sensor	x1
Ultrasonic sensor	x1
Joystick	x1
LCD	x1
330Ω resistance	x3
2kΩ resistance	x1

Requirements

One of the LEDs must be connected to a digital input (pin 1 to pin 13) in order to be able to modify the LED intensity using pulse-width modulation (PWM).
The button must be connected in the pin 2 or 3, those pins are associated to the interruptions.
The joystick have 2 potentiometer (X and Y) that generate analogic signals so the pins related to the X and Y axis of the joystick must being connected to an analogic input.
The button of the joystick must be connected to a digital input in order to be able to declare the button as a pull-up input.

Coding

Libraries

During this proyect we need to use multiple libraries.

This library is related to the implementation and management of threads. ```

include


- This [library](https://www.arduino.cc/en/Reference/LiquidCrystal) is used to control the LCD.

include


- This [library](https://github.com/adafruit/DHT-sensor-library) is necessary to interact with the DHT sensor.

include


- This [library](https://www.arduino.cc/reference/en/libraries/timerone/) is used to set and control timers.

include "TimerOne.h"

- This [library](https://github.com/vancegroup-mirrors/avr-libc/blob/master/avr-libc/include/avr/wdt.h) is used to set and control the watchdog.

include

### Threads
To make this project i used 5 differents thread associated to periodic tasks.
- Show distance in real time.

```c


// Creation of threads
Thread show_distanceThread = Thread();

void setup(){
  Serial.begin(9600);

  // show_distance Thread config
  show_distanceThread.enabled = true;
  show_distanceThread.setInterval(200);
  show_distanceThread.onRun(callback_measure_distance);
}

void callback_measure_distance(){
  // Send a 10us pulse
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Calculate the distance in cm
  t = pulseIn(echoPin, HIGH);
  d = t/59;

  // Print on the LCD the actual disntance
  lcd.clear();
  lcd.print(d);
  lcd.print(" cm");
}

Show how much time has passed since the program started.



// Creation of threads
Thread clockThread = Thread();

void setup(){
  Serial.begin(9600);

   // clock Thread config
  clockThread.enabled = true;
  clockThread.setInterval(200);
  clockThread.onRun(callback_clock);
}

void callback_clock(){
  initial_time = millis();
  lcd.clear();
  lcd.print(initial_time);
}

Check if someone is near the machine (< 1m).



// Creation of threads
Thread check_distanceThread = Thread();

void setup(){
  Serial.begin(9600);

   // check_distance Thread config
  check_distanceThread.enabled = true;
  check_distanceThread.setInterval(2500);
  check_distanceThread.onRun(callback_check_distance);
}

void callback_check_distance() {
  // Send a 10us pulse
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Calculate the distance in cm
  t = pulseIn(echoPin, HIGH);
  d = t/59;
  lcd.clear();
  // If someone is < 1m from the sensor, distance = true
  if (d < 100){
    distance = true;
  }
  else{
    distance = false;
    show_temp = true;
  }

  return distance, show_temp;
}

Controller

For the next 2 threads we will need a controller, because one of the threads will show the temperature and the other will show the humidity, they must update themselves, but they must be executed interleaved.

Show temperature thread. ```c

// Creation of threads
Thread measure_tempThread = Thread();

void setup(){
Serial.begin(9600);

// measure_temp Thread config
measure_tempThread.enabled = true;
measure_tempThread.setInterval(1000);
measure_tempThread.onRun(callback_measure_temp);
}

void callback_measure_temp() {
// Lógica para medir la temperatura aquí
temperature = dht.readTemperature();
lcd.clear();
lcd.print("T: ");
lcd.print(temperature);
lcd.print("C");
delay(1000);
}

- Show humidity thread.
```c


// Creation of threads
Thread measure_humThread = Thread();

void setup(){
  Serial.begin(9600);

   // measure_hum Thread config
  measure_humThread.enabled = true;
  measure_humThread.setInterval(1000);
  measure_humThread.onRun(callback_measure_hum);
}

void callback_measure_hum() {
  // Lógica para medir la humedad aquí
  humidity = dht.readHumidity();
  lcd.clear();
  lcd.print("H: ");
  lcd.print(humidity);
  lcd.print("%");
}

Controller for the two threads. ```c

// Creation of the controller
ThreadController controller = ThreadController();

void setup(){
Serial.begin(9600);

// adding measure_temp Thread and measure_hum Thread to the controller
controller.add(&measure_tempThread);
controller.add(&measure_humThread);
}

void admin_temp(){
while (analogRead(pinX) > 400){
controller.run();
}
}

### Interruption
I used interruptions to be able to enter the admin mode and reset while serving a coffee.
I used 2 types of interruptions:
- Hardware interruption.
```c


void setup() {
  Serial.begin(9600);
  attachInterrupt(digitalPinToInterrupt(BUTTON), button_pressed, CHANGE);
}

Software interruption.



void setup() {
  Serial.begin(9600);
  Timer1.initialize(1000000);  
  Timer1.attachInterrupt(callback_count);
}

Both interruptions: ```c

void button_pressed() {
// Enable interruptions
interrupts();

// Enable the watchdog (8sec)
wdt_enable(WDTO_8S);

// If the button is not pressed
if (digitalRead(BUTTON) == HIGH) {
wdt_disable();

// Stops the timer
Timer1.stop();

// Depending on the time and where were you before entering the interruption does a thing or an other
if (pressed_time >= 2 && pressed_time <= 3) {
  // Reset pressed_time
  pressed_time = 0;
  Serial.println("Botón pulsado entre 2 y 3 segundos");
  if (check_serve == true){
    check_serve = false;
    menu();
  }
} 
else if (pressed_time >= 5) {
  // Reset pressed_time
  pressed_time = 0;
  Serial.println("Botón pulsado más de 5 segundos");
  if (check_admin == false){
    admin();
  }
  else if (check_admin == true){
    analogWrite(LED2, 0);
    digitalWrite(LED1, LOW);
    menu();
  }
}
// Reset pressed_time
pressed_time = 0;

}
else {
// Enable interruptions
interrupts();

// Enable watchdog(8s)
wdt_enable(WDTO_8S);

// Start the timer
Timer1.start();

}
}
void callback_count() {
// Disable interruptions
noInterrupts();

pressed_time++;
Serial.println(pressed_time);
}

#### Variables

To modify variables that are inside interruptions, we need to declare them as volatile.



volatile bool check_admin = false;
volatile int pressed_time;
volatile bool check_serve = false;


</code></pre></div><h4>
  <a name="timerone" href="#timerone">
  </a>
  TimerOne
</h4>

<p>We need to use TimerOne library because the Timer0 of the Arduino board doesn´t work while it´s inside an interruption, so, to count how many seconds have you been pressing the button we need it.</p>
<h3>
  <a name="watchdog" href="#watchdog">
  </a>
  Watchdog
</h3>

<p>This program has a watchdog in the interruptions, in case something has blocked or is not responding to the behavior wanted, the watchdog resets the system after 8 seconds.</p>
<div class="highlight"><pre class="highlight c"><code>

<span class="kt">void</span> <span class="nf">setup</span><span class="p">()</span> <span class="p">{</span>
  <span class="n">Serial</span><span class="p">.</span><span class="n">begin</span><span class="p">(</span><span class="mi">9600</span><span class="p">);</span>
  <span class="c1">// Disable the watchdog</span>
  <span class="n">wdt_disable</span><span class="p">();</span>
<span class="p">}</span>

<span class="kt">void</span> <span class="nf">button_pressed</span><span class="p">()</span> <span class="p">{</span>
  <span class="c1">// Enable the watchdog (8sec)</span>
  <span class="n">wdt_enable</span><span class="p">(</span><span class="n">WDTO_8S</span><span class="p">);</span>
<span class="p">}</span>


</code></pre></div><h3>
  <a name="functionality" href="#functionality">
  </a>
  Functionality
</h3>

<ul>
<li>Client mode.
<iframe
  width="710"
  height="399"
  src="https://www.youtube.com/embed/roYreZuSSTw"
  allowfullscreen
  loading="lazy">
</iframe>

(In case the video doesn´t play try this link: <a href="https://youtu.be/roYreZuSSTw">https://youtu.be/roYreZuSSTw</a>)</li>
<li>Admin mode.
<iframe
  width="710"
  height="399"
  src="https://www.youtube.com/embed/-UY6RScsZmc"
  allowfullscreen
  loading="lazy">
</iframe>

(In case the video doesn´t play try this link: <a href="https://youtu.be/-UY6RScsZmc">https://youtu.be/-UY6RScsZmc</a>)</li>
</ul>

<p><strong>Complete code:</strong> <a href="https://github.com/JMMinguez/Vending_Machine_Controller">https://github.com/JMMinguez/Vending_Machine_Controller</a></p>

SmartRobot FollowLine & IoT

Jorge Martin — Wed, 11 Sep 2024 12:13:10 +0000

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.

Message of Arduino to ESP32:

Serial.print(3);
Serial.println(ultrasonic_distance);

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

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)