From 3f7af4ecc1b87c3e553417ad52eb4ff7970948f2 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Fri, 13 Mar 2026 12:55:08 +0100 Subject: [PATCH 01/17] dotbot/dotbot_simulator: add logic to load custom C based control loop function --- dotbot/dotbot_simulator.py | 83 +++++++++++++++++++++++++++++++++++--- 1 file changed, 78 insertions(+), 5 deletions(-) diff --git a/dotbot/dotbot_simulator.py b/dotbot/dotbot_simulator.py index 3cfb1c7..e644036 100644 --- a/dotbot/dotbot_simulator.py +++ b/dotbot/dotbot_simulator.py @@ -6,6 +6,7 @@ """Dotbot simulator for the DotBot project.""" +import ctypes import queue import random import threading @@ -13,6 +14,7 @@ from dataclasses import dataclass from enum import Enum from math import atan2, cos, pi, sin, sqrt +from pathlib import Path from typing import Callable, List import toml @@ -85,6 +87,22 @@ class SimulatedDotBotSettings(BaseModel): calibrated: int = 0xFF motor_left_error: float = 0 motor_right_error: float = 0 + custom_control_loop_library: Path = None + + +class RobotControl(ctypes.Structure): + _fields_ = [ + ("pos_x", ctypes.c_uint32), + ("pos_y", ctypes.c_uint32), + ("direction", ctypes.c_float), + ("waypoints_length", ctypes.c_uint8), + ("waypoint_idx", ctypes.c_uint8), + ("waypoint_x", ctypes.c_uint32), + ("waypoint_y", ctypes.c_uint32), + ("waypoint_threshold", ctypes.c_uint32), + ("pwm_left", ctypes.c_int8), + ("pwm_right", ctypes.c_int8), + ] class SimulatedNetworkSettings(BaseModel): @@ -106,6 +124,8 @@ def __init__(self, settings: SimulatedDotBotSettings, tx_queue: queue.Queue): self.theta = settings.theta self.motor_left_error = settings.motor_left_error self.motor_right_error = settings.motor_right_error + self.custom_control_loop_library = settings.custom_control_loop_library + self._control_loop_func = self._init_control_loop() self.time_elapsed_s = 0 self.pwm_left = 0 @@ -189,10 +209,62 @@ def update_state(self): if is_stopped: break - def _compute_automatic_control(self): - if self.controller_mode != DotBotSimulatorMode.AUTOMATIC: + def _init_control_loop(self) -> callable: + """Initialize the control loop, potentially loading a custom control loop library.""" + if self.custom_control_loop_library is not None: + self.custom_control_loop_library = ctypes.CDLL( + self.custom_control_loop_library + ) + self.custom_robot_control = RobotControl() + self.custom_robot_control.waypoint_idx = 0 + return self._control_loop_custom + else: + return self._control_loop_default + + def _control_loop_custom(self): + """Control loop using a custom control loop library.""" + if self.custom_control_loop_library is None: return + self.custom_robot_control.pos_x = int(self.pos_x) + self.custom_robot_control.pos_y = int(self.pos_y) + self.custom_robot_control.direction = self.theta + self.custom_robot_control.waypoints_length = len(self.waypoints) + if self.custom_robot_control.waypoint_idx < len(self.waypoints): + self.custom_robot_control.waypoint_x = int( + self.waypoints[self.custom_robot_control.waypoint_idx].pos_x + ) + self.custom_robot_control.waypoint_y = int( + self.waypoints[self.custom_robot_control.waypoint_idx].pos_y + ) + else: + self.custom_robot_control.waypoint_idx = 0 + self.pwm_left = 0 + self.pwm_right = 0 + self.controller_mode = DotBotSimulatorMode.MANUAL + self.custom_robot_control.waypoint_threshold = int(self.waypoint_threshold) + + # Call the custom control loop function from the shared library + self.custom_control_loop_library.update_control( + ctypes.byref(self.custom_robot_control) + ) + # Update the PWM values based on the output of the custom control loop + self.pwm_left = self.custom_robot_control.pwm_left + self.pwm_right = self.custom_robot_control.pwm_right + + self.logger.info( + "Custom control loop update", + v_left=self.pwm_left, + v_right=self.pwm_right, + theta=self.theta, + waypoint_index=self.custom_robot_control.waypoint_idx, + waypoints_length=self.custom_robot_control.waypoints_length, + waypoint_threshold=self.custom_robot_control.waypoint_threshold, + waypoint_x=self.custom_robot_control.waypoint_x, + waypoint_y=self.custom_robot_control.waypoint_y, + ) + + def _control_loop_default(self): delta_x = self.pos_x - self.waypoints[self.waypoint_index].pos_x delta_y = self.pos_y - self.waypoints[self.waypoint_index].pos_y distance_to_target = sqrt(delta_x**2 + delta_y**2) @@ -239,7 +311,7 @@ def _compute_automatic_control(self): self.pwm_right = MOTOR_SPEED * speed_reduction_factor - angular_speed self.logger.info( - "Control loop update", + "Default control loop update", v_left=self.pwm_left, v_right=self.pwm_right, theta=self.theta, @@ -248,8 +320,9 @@ def _compute_automatic_control(self): def control_thread(self): """Control thread to update the state of the dotbot simulator.""" while self._stop_event.is_set() is False: - with self._lock: - self._compute_automatic_control() + if self.controller_mode == DotBotSimulatorMode.AUTOMATIC: + with self._lock: + self._control_loop_func() is_stopped = self._stop_event.wait(SIMULATOR_UPDATE_INTERVAL_S) if is_stopped: break From f2a15cfb246a9302b166c8637591be60a848a6fd Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Tue, 17 Mar 2026 17:16:32 +0100 Subject: [PATCH 02/17] dotbot/dotbot_simulator: refactor to use angles in degrees, fix orientation --- dotbot/dotbot_simulator.py | 118 ++++++++++++++++++------------------- simulator_init_state.toml | 8 +-- 2 files changed, 63 insertions(+), 63 deletions(-) diff --git a/dotbot/dotbot_simulator.py b/dotbot/dotbot_simulator.py index e644036..ae53d5e 100644 --- a/dotbot/dotbot_simulator.py +++ b/dotbot/dotbot_simulator.py @@ -33,7 +33,7 @@ # Control parameters for the automatic mode MOTOR_SPEED = 60 -ANGULAR_SPEED_GAIN = 1.2 +ANGULAR_SPEED_GAIN = 50 REDUCE_SPEED_FACTOR = 0.7 REDUCE_SPEED_ANGLE = 25 @@ -94,7 +94,7 @@ class RobotControl(ctypes.Structure): _fields_ = [ ("pos_x", ctypes.c_uint32), ("pos_y", ctypes.c_uint32), - ("direction", ctypes.c_float), + ("direction", ctypes.c_int16), ("waypoints_length", ctypes.c_uint8), ("waypoint_idx", ctypes.c_uint8), ("waypoint_x", ctypes.c_uint32), @@ -179,24 +179,24 @@ def _diff_drive_model_update(self): 1 - self.motor_right_error ) - V = (v_left_real + v_right_real) / 2 - w = (v_left_real - v_right_real) / L - x_dot = V * cos(theta_old - pi) - y_dot = V * sin(theta_old - pi) + V = (v_right_real + v_left_real) / 2 + w = (v_right_real - v_left_real) / L + x_dot = V * cos(theta_old * pi / 180 - pi / 2) + y_dot = V * sin(theta_old * pi / 180 + pi / 2) self.pos_x = pos_x_old + x_dot * SIMULATOR_STEP_DELTA_T self.pos_y = pos_y_old + y_dot * SIMULATOR_STEP_DELTA_T - self.theta = (theta_old + w * SIMULATOR_STEP_DELTA_T) % (2 * pi) + self.theta = (theta_old + w * SIMULATOR_STEP_DELTA_T * 180 / pi) % 360 self.logger.debug( "State updated", - pos_x=self.pos_x, - pos_y=self.pos_y, - theta=self.theta, - pwm_left=self.pwm_left, - pwm_right=self.pwm_right, - v_left_real=v_left_real, - v_right_real=v_right_real, + pos_x=int(self.pos_x), + pos_y=int(self.pos_y), + theta=int(self.theta), + pwm_left=int(self.pwm_left), + pwm_right=int(self.pwm_right), + v_left_real=int(v_left_real), + v_right_real=int(v_right_real), ) self.time_elapsed_s += SIMULATOR_STEP_DELTA_T @@ -227,7 +227,7 @@ def _control_loop_custom(self): return self.custom_robot_control.pos_x = int(self.pos_x) self.custom_robot_control.pos_y = int(self.pos_y) - self.custom_robot_control.direction = self.theta + self.custom_robot_control.direction = int(self.theta * -1) self.custom_robot_control.waypoints_length = len(self.waypoints) if self.custom_robot_control.waypoint_idx < len(self.waypoints): self.custom_robot_control.waypoint_x = int( @@ -253,9 +253,9 @@ def _control_loop_custom(self): self.pwm_right = self.custom_robot_control.pwm_right self.logger.info( - "Custom control loop update", - v_left=self.pwm_left, - v_right=self.pwm_right, + "Custom loop", + pwm_left=self.pwm_left, + pwm_right=self.pwm_right, theta=self.theta, waypoint_index=self.custom_robot_control.waypoint_idx, waypoints_length=self.custom_robot_control.waypoints_length, @@ -265,8 +265,8 @@ def _control_loop_custom(self): ) def _control_loop_default(self): - delta_x = self.pos_x - self.waypoints[self.waypoint_index].pos_x - delta_y = self.pos_y - self.waypoints[self.waypoint_index].pos_y + delta_x = self.waypoints[self.waypoint_index].pos_x - self.pos_x + delta_y = self.waypoints[self.waypoint_index].pos_y - self.pos_y distance_to_target = sqrt(delta_x**2 + delta_y**2) # check if we are close enough to the "next" waypoint @@ -282,39 +282,41 @@ def _control_loop_default(self): self.pwm_right = 0 self.waypoint_index = 0 self.controller_mode = DotBotSimulatorMode.MANUAL - else: - robot_angle = self.theta - angle_to_target = atan2(delta_y, delta_x) - if robot_angle >= pi: - robot_angle = robot_angle - 2 * pi - - error_angle = ((angle_to_target - robot_angle + pi) % (2 * pi)) - pi - self.logger.info( - "Moving to waypoint", - robot_angle=robot_angle, - angle_to_target=angle_to_target, - error_angle=error_angle, - ) - - speed_reduction_factor: float = 1.0 - if distance_to_target < self.waypoint_threshold * 2: - speed_reduction_factor = REDUCE_SPEED_FACTOR - - error_angle_degrees = error_angle * 180 / pi - if ( - error_angle_degrees > REDUCE_SPEED_ANGLE - or error_angle_degrees < -REDUCE_SPEED_ANGLE - ): - speed_reduction_factor = REDUCE_SPEED_FACTOR - angular_speed = error_angle * MOTOR_SPEED * ANGULAR_SPEED_GAIN - self.pwm_left = MOTOR_SPEED * speed_reduction_factor + angular_speed - self.pwm_right = MOTOR_SPEED * speed_reduction_factor - angular_speed + return + + angle_to_target = -1 * atan2(delta_x, delta_y) * 180 / pi + robot_angle = self.theta * -1 + if robot_angle >= 180: + robot_angle -= 360 + elif robot_angle < -180: + robot_angle += 360 + + error_angle = angle_to_target - robot_angle + if error_angle >= 180: + error_angle -= 360 + elif error_angle < -180: + error_angle += 360 + + speed_reduction_factor: float = 1.0 + if distance_to_target < self.waypoint_threshold * 2: + speed_reduction_factor = REDUCE_SPEED_FACTOR + if error_angle > REDUCE_SPEED_ANGLE or error_angle < -REDUCE_SPEED_ANGLE: + speed_reduction_factor = REDUCE_SPEED_FACTOR + + angular_speed = (error_angle / 180) * ANGULAR_SPEED_GAIN + self.pwm_left = MOTOR_SPEED * speed_reduction_factor + angular_speed + self.pwm_right = MOTOR_SPEED * speed_reduction_factor - angular_speed self.logger.info( - "Default control loop update", - v_left=self.pwm_left, - v_right=self.pwm_right, - theta=self.theta, + "Loop update", + robot_angle=int(robot_angle), + angle_to_target=int(angle_to_target), + error_angle=int(error_angle), + angular_speed=int(angular_speed), + pwm_left=int(self.pwm_left), + pwm_right=int(self.pwm_right), + theta=int(self.theta), + waypoint=f"{self.waypoint_index}/{len(self.waypoints)}", ) def control_thread(self): @@ -335,7 +337,7 @@ def advertise(self): packet=Packet.from_payload( PayloadDotBotAdvertisement( calibrated=self.calibrated, - direction=int(self.theta * 180 / pi + 90), + direction=int(self.theta * -1), pos_x=int(self.pos_x) if self.pos_x >= 0 else 0, pos_y=int(self.pos_y) if self.pos_y >= 0 else 0, pos_z=0, @@ -361,17 +363,15 @@ def rx_frame(self): self.controller_mode = DotBotSimulatorMode.MANUAL self.pwm_left = frame.packet.payload.left_y self.pwm_right = frame.packet.payload.right_y - self.logger.info( - "RAW command received", - v_left=self.pwm_left, - v_right=self.pwm_right, - ) - if self.pwm_left > 127: self.pwm_left = self.pwm_left - 256 if self.pwm_right > 127: self.pwm_right = self.pwm_right - 256 - + self.logger.info( + "RAW command received", + pwm_left=self.pwm_left, + pwm_right=self.pwm_right, + ) elif frame.payload_type == PayloadType.LH2_WAYPOINTS: self.pwm_left = 0 self.pwm_right = 0 diff --git a/simulator_init_state.toml b/simulator_init_state.toml index 6c09d66..358b6f6 100644 --- a/simulator_init_state.toml +++ b/simulator_init_state.toml @@ -6,22 +6,22 @@ address = "BADCAFE111111111" # DotBot unique address calibrated = 0xff # optional, defaults to only first lighthouse calibrated pos_x = 400 # [0, 2_000] in mm pos_y = 400 # [0, 2_000] -theta = 0.0 # [0.0, 2pi] +theta = 0 # [0, 360] in degrees, 0 is facing north, increasing clockwise [[dotbots]] address = "DEADBEEF22222222" pos_x = 400 pos_y = 200 -theta = 1.57 +theta = 180 [[dotbots]] address = "B0B0F00D33333333" pos_x = 1_500 pos_y = 1_500 -theta = 1.57 +theta = 180 [[dotbots]] address = "BADC0DE444444444" pos_x = 1_000 pos_y = 1_000 -theta = 3.14 +theta = 360 From 891079e830fb6a95e1662ce08f7bea4957d7bf76 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Tue, 17 Mar 2026 17:17:06 +0100 Subject: [PATCH 03/17] dotbot/frontend: fix bug not showing robot direction if direction is 0 --- dotbot/frontend/src/DotBotsMap.js | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/dotbot/frontend/src/DotBotsMap.js b/dotbot/frontend/src/DotBotsMap.js index 4b08bbc..f80da0b 100644 --- a/dotbot/frontend/src/DotBotsMap.js +++ b/dotbot/frontend/src/DotBotsMap.js @@ -91,7 +91,7 @@ const DotBotsMapPoint = React.memo((props) => { const posX = props.mapSize * parseInt(props.dotbot.lh2_position.x) / props.areaSize.width; const posY = props.mapSize * parseInt(props.dotbot.lh2_position.y) / props.areaSize.width; - const rotation = (props.dotbot.direction) ? props.dotbot.direction : 0; + const rotation = (props.dotbot.direction !== -1000) ? props.dotbot.direction : 0; const radius = (props.dotbot.address === props.active || hovered) ? props.mapSize * (dotbotRadius + 5) / props.areaSize.width : props.mapSize * dotbotRadius / props.areaSize.width; const directionShift = (props.dotbot.address === props.active || hovered) ? 2: 1; const directionSize = (props.dotbot.address === props.active || hovered) ? props.mapSize * (dotbotRadius + 5) / props.areaSize.width : props.mapSize * dotbotRadius / props.areaSize.width; @@ -134,12 +134,12 @@ const DotBotsMapPoint = React.memo((props) => { )) )} - { props.updateActive(props.dotbot.address === props.active ? inactiveAddress : props.dotbot.address) @@ -149,10 +149,11 @@ const DotBotsMapPoint = React.memo((props) => { onMouseLeave={onMouseLeave} > {`${props.dotbot.address}@${posX}x${posY}`} - {(props.dotbot.direction) && + {(props.dotbot.direction !== -1000) && + - } + } ) }) From 69a3fb47ae4c2a784df9108b6a8c61199af7e998 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Tue, 17 Mar 2026 17:21:15 +0100 Subject: [PATCH 04/17] doc: add new external Python class to ignored list --- doc/conf.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/conf.py b/doc/conf.py index 88e624b..e17162f 100644 --- a/doc/conf.py +++ b/doc/conf.py @@ -46,6 +46,8 @@ ("py:class", r"Query"), ("py:class", r"PydanticUndefined"), ("py:class", r"queue.Queue"), + ("py:class", r"_ctypes.Structure"), + ("py:class", r"pathlib._local.Path"), ] # -- Options for HTML output ------------------------------------------------- From 4d7f39e90fddaca1a193a352f40bc5293bea9287 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Thu, 19 Mar 2026 14:03:38 +0100 Subject: [PATCH 05/17] dotbot/dotbot_simulator: refactor direction management --- dotbot/dotbot_simulator.py | 84 +++++++++++++++++++++----------------- simulator_init_state.toml | 8 ++-- 2 files changed, 51 insertions(+), 41 deletions(-) diff --git a/dotbot/dotbot_simulator.py b/dotbot/dotbot_simulator.py index ae53d5e..f8cdf98 100644 --- a/dotbot/dotbot_simulator.py +++ b/dotbot/dotbot_simulator.py @@ -33,9 +33,10 @@ # Control parameters for the automatic mode MOTOR_SPEED = 60 -ANGULAR_SPEED_GAIN = 50 +# ANGULAR_SPEED_GAIN = 50 REDUCE_SPEED_FACTOR = 0.7 REDUCE_SPEED_ANGLE = 25 +DIRECTION_THRESHOLD = 50 # threshold to update the direction (50mm) SIMULATOR_STEP_DELTA_T = 0.02 # 20 ms @@ -61,6 +62,10 @@ def wheel_speed_from_pwm(pwm: float) -> float: """Convert a PWM value to a wheel speed in mm/s.""" if abs(pwm) < MIN_PWM_TO_MOVE: return 0.0 + if pwm > 100: + pwm = 100 + if pwm < -100: + pwm = -100 return pwm * R * Kv / (r * 127) @@ -83,7 +88,7 @@ class SimulatedDotBotSettings(BaseModel): address: str pos_x: int pos_y: int - theta: float + direction: int = -1000 calibrated: int = 0xFF motor_left_error: float = 0 motor_right_error: float = 0 @@ -121,7 +126,7 @@ def __init__(self, settings: SimulatedDotBotSettings, tx_queue: queue.Queue): self.address = settings.address.lower() self.pos_x = settings.pos_x self.pos_y = settings.pos_y - self.theta = settings.theta + self.theta = settings.direction * -1 if settings.direction != -1000 else 0 self.motor_left_error = settings.motor_left_error self.motor_right_error = settings.motor_right_error self.custom_control_loop_library = settings.custom_control_loop_library @@ -130,6 +135,7 @@ def __init__(self, settings: SimulatedDotBotSettings, tx_queue: queue.Queue): self.pwm_left = 0 self.pwm_right = 0 + self.direction = settings.direction self.calibrated = settings.calibrated self.waypoint_threshold = 0 @@ -150,6 +156,13 @@ def __init__(self, settings: SimulatedDotBotSettings, tx_queue: queue.Queue): self.advertise_thread.start() self.control_thread.start() self.main_thread.start() + self.logger.info( + "DotBot simulator initialized", + pos_x=self.pos_x, + pos_y=self.pos_y, + direction=self.direction, + theta=self.theta, + ) @property def header(self): @@ -164,15 +177,6 @@ def _diff_drive_model_update(self): pos_y_old = self.pos_y theta_old = self.theta - if self.pwm_left > 100: - self.pwm_left = 100 - if self.pwm_right > 100: - self.pwm_right = 100 - if self.pwm_left < -100: - self.pwm_left = -100 - if self.pwm_right < -100: - self.pwm_right = -100 - # Compute each wheel's real speed considering the motor error and the minimum PWM to move v_left_real = wheel_speed_from_pwm(self.pwm_left) * (1 - self.motor_left_error) v_right_real = wheel_speed_from_pwm(self.pwm_right) * ( @@ -183,20 +187,24 @@ def _diff_drive_model_update(self): w = (v_right_real - v_left_real) / L x_dot = V * cos(theta_old * pi / 180 - pi / 2) y_dot = V * sin(theta_old * pi / 180 + pi / 2) + dx = x_dot * SIMULATOR_STEP_DELTA_T + dy = y_dot * SIMULATOR_STEP_DELTA_T - self.pos_x = pos_x_old + x_dot * SIMULATOR_STEP_DELTA_T - self.pos_y = pos_y_old + y_dot * SIMULATOR_STEP_DELTA_T + self.pos_x = pos_x_old + dx + self.pos_y = pos_y_old + dy self.theta = (theta_old + w * SIMULATOR_STEP_DELTA_T * 180 / pi) % 360 + if sqrt(dx**2 + dy**2): + self.direction = int(-1 * atan2(dx, dy) * 180 / pi) % 360 + self.logger.debug( "State updated", pos_x=int(self.pos_x), pos_y=int(self.pos_y), theta=int(self.theta), + direction=int(self.direction), pwm_left=int(self.pwm_left), pwm_right=int(self.pwm_right), - v_left_real=int(v_left_real), - v_right_real=int(v_right_real), ) self.time_elapsed_s += SIMULATOR_STEP_DELTA_T @@ -223,26 +231,17 @@ def _init_control_loop(self) -> callable: def _control_loop_custom(self): """Control loop using a custom control loop library.""" - if self.custom_control_loop_library is None: - return self.custom_robot_control.pos_x = int(self.pos_x) self.custom_robot_control.pos_y = int(self.pos_y) - self.custom_robot_control.direction = int(self.theta * -1) + self.custom_robot_control.direction = self.direction self.custom_robot_control.waypoints_length = len(self.waypoints) - if self.custom_robot_control.waypoint_idx < len(self.waypoints): - self.custom_robot_control.waypoint_x = int( - self.waypoints[self.custom_robot_control.waypoint_idx].pos_x - ) - self.custom_robot_control.waypoint_y = int( - self.waypoints[self.custom_robot_control.waypoint_idx].pos_y - ) - else: - self.custom_robot_control.waypoint_idx = 0 - self.pwm_left = 0 - self.pwm_right = 0 - self.controller_mode = DotBotSimulatorMode.MANUAL + self.custom_robot_control.waypoint_x = int( + self.waypoints[self.custom_robot_control.waypoint_idx].pos_x + ) + self.custom_robot_control.waypoint_y = int( + self.waypoints[self.custom_robot_control.waypoint_idx].pos_y + ) self.custom_robot_control.waypoint_threshold = int(self.waypoint_threshold) - # Call the custom control loop function from the shared library self.custom_control_loop_library.update_control( ctypes.byref(self.custom_robot_control) @@ -252,11 +251,19 @@ def _control_loop_custom(self): self.pwm_left = self.custom_robot_control.pwm_left self.pwm_right = self.custom_robot_control.pwm_right + if self.custom_robot_control.waypoint_idx >= len(self.waypoints): + self.logger.info("Last waypoint reached") + self.pwm_left = 0 + self.pwm_right = 0 + self.waypoint_index = 0 + self.custom_robot_control.waypoint_idx = 0 + self.controller_mode = DotBotSimulatorMode.MANUAL + self.logger.info( "Custom loop", pwm_left=self.pwm_left, pwm_right=self.pwm_right, - theta=self.theta, + direction=self.direction, waypoint_index=self.custom_robot_control.waypoint_idx, waypoints_length=self.custom_robot_control.waypoints_length, waypoint_threshold=self.custom_robot_control.waypoint_threshold, @@ -284,8 +291,11 @@ def _control_loop_default(self): self.controller_mode = DotBotSimulatorMode.MANUAL return - angle_to_target = -1 * atan2(delta_x, delta_y) * 180 / pi - robot_angle = self.theta * -1 + if distance_to_target < DIRECTION_THRESHOLD: + angle_to_target = 0 # move forward a bit + else: + angle_to_target = -1 * atan2(delta_x, delta_y) * 180 / pi + robot_angle = self.direction if robot_angle >= 180: robot_angle -= 360 elif robot_angle < -180: @@ -303,7 +313,7 @@ def _control_loop_default(self): if error_angle > REDUCE_SPEED_ANGLE or error_angle < -REDUCE_SPEED_ANGLE: speed_reduction_factor = REDUCE_SPEED_FACTOR - angular_speed = (error_angle / 180) * ANGULAR_SPEED_GAIN + angular_speed = (error_angle / 180) * MOTOR_SPEED self.pwm_left = MOTOR_SPEED * speed_reduction_factor + angular_speed self.pwm_right = MOTOR_SPEED * speed_reduction_factor - angular_speed @@ -337,7 +347,7 @@ def advertise(self): packet=Packet.from_payload( PayloadDotBotAdvertisement( calibrated=self.calibrated, - direction=int(self.theta * -1), + direction=self.direction, pos_x=int(self.pos_x) if self.pos_x >= 0 else 0, pos_y=int(self.pos_y) if self.pos_y >= 0 else 0, pos_z=0, diff --git a/simulator_init_state.toml b/simulator_init_state.toml index 358b6f6..4869d43 100644 --- a/simulator_init_state.toml +++ b/simulator_init_state.toml @@ -6,22 +6,22 @@ address = "BADCAFE111111111" # DotBot unique address calibrated = 0xff # optional, defaults to only first lighthouse calibrated pos_x = 400 # [0, 2_000] in mm pos_y = 400 # [0, 2_000] -theta = 0 # [0, 360] in degrees, 0 is facing north, increasing clockwise +direction = 0 # [0, 360] in degrees, 0 is facing north, increasing clockwise [[dotbots]] address = "DEADBEEF22222222" pos_x = 400 pos_y = 200 -theta = 180 +direction = 180 [[dotbots]] address = "B0B0F00D33333333" pos_x = 1_500 pos_y = 1_500 -theta = 180 +direction = 180 [[dotbots]] address = "BADC0DE444444444" pos_x = 1_000 pos_y = 1_000 -theta = 360 +direction = 360 From aab8c8d501cf5ceb9de25098d40b532ee623b727 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Thu, 19 Mar 2026 16:33:23 +0100 Subject: [PATCH 06/17] dotbot/dotbot_simulator: improve default control loop to better match the real one --- dotbot/dotbot_simulator.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dotbot/dotbot_simulator.py b/dotbot/dotbot_simulator.py index f8cdf98..dd733ba 100644 --- a/dotbot/dotbot_simulator.py +++ b/dotbot/dotbot_simulator.py @@ -27,14 +27,14 @@ Kv = 400 # motor speed constant in RPM r = 50 # motor reduction ratio -R = 25 # wheel radius in mm +R = 50 # wheel diameter in mm L = 70 # distance between the two wheels in mm MIN_PWM_TO_MOVE = 30 # minimum PWM value to overcome static friction and start moving # Control parameters for the automatic mode MOTOR_SPEED = 60 -# ANGULAR_SPEED_GAIN = 50 -REDUCE_SPEED_FACTOR = 0.7 +ANGULAR_SPEED_GAIN = 0.6 +REDUCE_SPEED_FACTOR = 0.8 REDUCE_SPEED_ANGLE = 25 DIRECTION_THRESHOLD = 50 # threshold to update the direction (50mm) @@ -313,7 +313,7 @@ def _control_loop_default(self): if error_angle > REDUCE_SPEED_ANGLE or error_angle < -REDUCE_SPEED_ANGLE: speed_reduction_factor = REDUCE_SPEED_FACTOR - angular_speed = (error_angle / 180) * MOTOR_SPEED + angular_speed = (error_angle / 180) * MOTOR_SPEED * ANGULAR_SPEED_GAIN self.pwm_left = MOTOR_SPEED * speed_reduction_factor + angular_speed self.pwm_right = MOTOR_SPEED * speed_reduction_factor - angular_speed From 0c5ed9a99211771cdcfd755998193f7d3671c1df Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Thu, 19 Mar 2026 16:47:36 +0100 Subject: [PATCH 07/17] doc: fix build --- doc/conf.py | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/conf.py b/doc/conf.py index e17162f..52eb0e2 100644 --- a/doc/conf.py +++ b/doc/conf.py @@ -48,6 +48,7 @@ ("py:class", r"queue.Queue"), ("py:class", r"_ctypes.Structure"), ("py:class", r"pathlib._local.Path"), + ("py:class", r"pathlib.Path"), ] # -- Options for HTML output ------------------------------------------------- From a513149a1d5ba0d435a7fbbd910d0b58862e24d8 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Fri, 20 Mar 2026 11:06:51 +0100 Subject: [PATCH 08/17] utils/control_loop: add CMake project and documentation to build custom control loop library --- utils/control_loop/.gitignore | 1 + utils/control_loop/CMakeLists.txt | 34 ++++++++++++++++++++++ utils/control_loop/README.md | 47 +++++++++++++++++++++++++++++++ 3 files changed, 82 insertions(+) create mode 100644 utils/control_loop/.gitignore create mode 100644 utils/control_loop/CMakeLists.txt create mode 100644 utils/control_loop/README.md diff --git a/utils/control_loop/.gitignore b/utils/control_loop/.gitignore new file mode 100644 index 0000000..567609b --- /dev/null +++ b/utils/control_loop/.gitignore @@ -0,0 +1 @@ +build/ diff --git a/utils/control_loop/CMakeLists.txt b/utils/control_loop/CMakeLists.txt new file mode 100644 index 0000000..3578431 --- /dev/null +++ b/utils/control_loop/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.10) +project(dotbot_control_loop VERSION 1.0 LANGUAGES C) + +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_BUILD_TYPE Release) + +# Set default dotbot version +if(NOT DEFINED DOTBOT_VERSION) + set(DOTBOT_VERSION 3) +endif() + +# Require DOTBOT_LIBS_DIR +if(NOT DEFINED DOTBOT_LIBS_DIR) + message(FATAL_ERROR "DOTBOT_LIBS_DIR must be set") +endif() + +# Create shared library +add_library(${PROJECT_NAME} SHARED + ${DOTBOT_LIBS_DIR}/drv/control_loop/control_loop.c +) + +# Set include directories +target_include_directories(${PROJECT_NAME} PRIVATE + ${DOTBOT_LIBS_DIR}/drv +) + +# Set preprocessor definition +target_compile_definitions(${PROJECT_NAME} PRIVATE + BOARD_DOTBOT_V${DOTBOT_VERSION} +) + +# Add fast-math flag +target_compile_options(${PROJECT_NAME} PRIVATE -ffast-math) diff --git a/utils/control_loop/README.md b/utils/control_loop/README.md new file mode 100644 index 0000000..7be1dbf --- /dev/null +++ b/utils/control_loop/README.md @@ -0,0 +1,47 @@ + +# Custom Control Loop Library + +## Overview + +This guide explains how to build a custom control loop library from the C +version provided by DotBot-libs and integrate it with the simulator using FFI +(Foreign Function Interface). + +## Building with CMake + +### Prerequisites + +- CMake 3.10 or higher +- C compiler (gcc, clang, or MSVC) + +### Build Steps + +```bash +cmake -DDOTBOT_LIBS_DIR= -DDOTBOT_VERSION= -B build . +make -C build +``` + +This creates the `build/` subdirectory and generates there the +`libdotbot_control_loop.so` library file. + +`DOTBOT_LIBS_DIR` variable must be set to tell the build system where to find +the dotbot control loop library. +If not set default value for `DOTBOT_VERSION` is 3. + + +3. The compiled shared library will be in the `build/` directory (`.so` on Linux, +`.dylib` on macOS, `.dll` on Windows). + +## Integration with Simulator + +### Configuration + +Add your custom control loop library to `simulator_init_state.toml`: + +```toml +[[robots]] +address = "DECAFBAD4B0B0AAA" +custom_control_loop_library = "/path/to/libdotbot_control_loop.so" +``` + +Each robot configuration can specify its own library independently. From 63ed07ee6cd12696f29c66d02587552b2fb2c898 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Fri, 20 Mar 2026 11:08:04 +0100 Subject: [PATCH 09/17] .github: add job to build custom control loop --- .github/workflows/continuous-integration.yml | 24 +++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/.github/workflows/continuous-integration.yml b/.github/workflows/continuous-integration.yml index 43eef5b..4d2786f 100644 --- a/.github/workflows/continuous-integration.yml +++ b/.github/workflows/continuous-integration.yml @@ -52,6 +52,28 @@ jobs: with: verbose: true + control_loop: + name: check custom control loop + runs-on: ${{ matrix.os }} + strategy: + fail-fast: false + matrix: + os: + - ubuntu-latest + - macos-latest + steps: + - name: Checkout repo + uses: actions/checkout@v4 + - name: Checkout DotBot-libs repo + uses: actions/checkout@v4 + with: + repository: DotBots/DotBot-libs + path: DotBot-libs + - name: Build control loop library + run: | + cmake -DDOTBOT_LIBS_DIR=${PWD}/DotBot-libs -S utils/control_loop -B build + cmake --build build + doc: name: check documentation runs-on: ubuntu-latest @@ -92,7 +114,7 @@ jobs: path: ./dotbot/frontend/build package: - needs: [test, doc, frontend] + needs: [test, doc, frontend, control_loop] name: build source package runs-on: ${{ matrix.os }} strategy: From 79efc9838e2f978b53c0397fa3af6c213f4b7d59 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Fri, 20 Mar 2026 13:27:47 +0100 Subject: [PATCH 10/17] dotbot/dotbot_simulator: improve pure python control loop behavior --- dotbot/dotbot_simulator.py | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/dotbot/dotbot_simulator.py b/dotbot/dotbot_simulator.py index dd733ba..bacbad8 100644 --- a/dotbot/dotbot_simulator.py +++ b/dotbot/dotbot_simulator.py @@ -26,14 +26,14 @@ from dotbot.protocol import PayloadDotBotAdvertisement, PayloadType Kv = 400 # motor speed constant in RPM -r = 50 # motor reduction ratio -R = 50 # wheel diameter in mm +R = 50 # motor reduction ratio +D = 50 # wheel diameter in mm L = 70 # distance between the two wheels in mm MIN_PWM_TO_MOVE = 30 # minimum PWM value to overcome static friction and start moving # Control parameters for the automatic mode MOTOR_SPEED = 60 -ANGULAR_SPEED_GAIN = 0.6 +ANGULAR_SPEED_GAIN = 2 REDUCE_SPEED_FACTOR = 0.8 REDUCE_SPEED_ANGLE = 25 DIRECTION_THRESHOLD = 50 # threshold to update the direction (50mm) @@ -66,7 +66,7 @@ def wheel_speed_from_pwm(pwm: float) -> float: pwm = 100 if pwm < -100: pwm = -100 - return pwm * R * Kv / (r * 127) + return pwm * D * Kv / (R * 127) class DotBotSimulatorMode(Enum): @@ -291,10 +291,7 @@ def _control_loop_default(self): self.controller_mode = DotBotSimulatorMode.MANUAL return - if distance_to_target < DIRECTION_THRESHOLD: - angle_to_target = 0 # move forward a bit - else: - angle_to_target = -1 * atan2(delta_x, delta_y) * 180 / pi + angle_to_target = -1 * atan2(delta_x, delta_y) * 180 / pi robot_angle = self.direction if robot_angle >= 180: robot_angle -= 360 @@ -383,9 +380,6 @@ def rx_frame(self): pwm_right=self.pwm_right, ) elif frame.payload_type == PayloadType.LH2_WAYPOINTS: - self.pwm_left = 0 - self.pwm_right = 0 - self.controller_mode = DotBotSimulatorMode.MANUAL self.waypoint_threshold = frame.packet.payload.threshold self.waypoints = frame.packet.payload.waypoints self.logger.info( @@ -395,6 +389,10 @@ def rx_frame(self): ) if self.waypoints: self.controller_mode = DotBotSimulatorMode.AUTOMATIC + else: + self.pwm_left = 0 + self.pwm_right = 0 + self.controller_mode = DotBotSimulatorMode.MANUAL def stop(self): self.logger.info(f"Stopping DotBot {self.address} simulator...") From b0ee90067c10967b0ee5cddf10f4e76702d0a075 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Fri, 20 Mar 2026 13:44:44 +0100 Subject: [PATCH 11/17] dotbot/examples/charging_station: reduce speed used to disengage from charger --- dotbot/examples/charging_station/charging_station.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dotbot/examples/charging_station/charging_station.py b/dotbot/examples/charging_station/charging_station.py index dfc0f30..2ba763f 100644 --- a/dotbot/examples/charging_station/charging_station.py +++ b/dotbot/examples/charging_station/charging_station.py @@ -149,7 +149,7 @@ async def disengage_from_charger(client: RestClient, dotbot_address: str): address=dotbot_address, application=dotbot.application, command=DotBotMoveRawCommandModel( - left_x=0, left_y=-100, right_x=0, right_y=-100 + left_x=0, left_y=-80, right_x=0, right_y=-80 ), ) await asyncio.sleep(0.1) @@ -164,7 +164,7 @@ async def disengage_from_charger(client: RestClient, dotbot_address: str): address=dotbot_address, application=dotbot.application, command=DotBotMoveRawCommandModel( - left_x=0, left_y=100, right_x=0, right_y=100 + left_x=0, left_y=80, right_x=0, right_y=80 ), ) await asyncio.sleep(0.1) From 8a1c5564e02aa23547d2b380ec9cce8dc26b7efe Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Fri, 20 Mar 2026 14:57:43 +0100 Subject: [PATCH 12/17] pyproject.toml: add rich dependency --- pyproject.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/pyproject.toml b/pyproject.toml index 57420ee..652b0db 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -40,6 +40,7 @@ dependencies = [ "pynput >= 1.8.1", "pyserial >= 3.5", "qrkey >= 0.12.1", + "rich >= 14.0.0", "structlog >= 24.4.0", "uvicorn >= 0.32.0", "websockets >= 13.1.0", From f6636e03266e487cebb4dbe9b69c045a9a658b60 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Fri, 20 Mar 2026 14:58:11 +0100 Subject: [PATCH 13/17] dotbot/examples/minimum_naming_game: refactor init pose generator --- .../minimum_naming_game/gen_init_pose.py | 123 +++++++++++++++--- .../minimum_naming_game/init_state.toml | 50 +++---- .../init_state_with_motion.toml | 54 ++++---- 3 files changed, 157 insertions(+), 70 deletions(-) diff --git a/dotbot/examples/minimum_naming_game/gen_init_pose.py b/dotbot/examples/minimum_naming_game/gen_init_pose.py index 1be4766..9418b22 100644 --- a/dotbot/examples/minimum_naming_game/gen_init_pose.py +++ b/dotbot/examples/minimum_naming_game/gen_init_pose.py @@ -2,20 +2,27 @@ import random from pathlib import Path -# --- Configuration --- -WIDTH_NODES = 5 # Robots per row -HEIGHT_NODES = 5 # Number of rows -SEP_X = 240 # Separation between columns -SEP_Y = 240 # Separation between rows - +import click +from rich import print -def format_with_underscores(value): - """Formats an integer with underscores every three digits.""" - return f"{value:_}" +# --- Configuration --- +WIDTH_NODES_DEFAULT = 5 # Robots per row +HEIGHT_NODES_DEFAULT = 5 # Number of rows +START_X_DEFAULT = 120 # Starting X position in mm +START_Y_DEFAULT = 120 # Starting Y position in mm +SEP_X_DEFAULT = 240 # Separation between columns +SEP_Y_DEFAULT = 240 # Separation between rows def generate_lattice_toml( - width_count, height_count, sep_x, sep_y, start_x=120, start_y=120 + width_count, + height_count, + start_x, + start_y, + sep_x, + sep_y, + direction=None, + control_loop_library_path=None, ): output_lines = [] @@ -28,8 +35,9 @@ def generate_lattice_toml( pos_x = start_x + (col * sep_x) pos_y = start_y + (row * sep_y) - # Randomize theta between 0 and 2*pi - random_theta = round(random.uniform(0, 2 * math.pi), 2) + # Randomize direction between 0 and 360 + if direction is None: + direction = random.randint(0, 360) # Manually build the TOML entry string to preserve underscores output_lines.append("[[dotbots]]") @@ -37,18 +45,97 @@ def generate_lattice_toml( output_lines.append("calibrated = 0xff") output_lines.append(f"pos_x = {pos_x:_}") output_lines.append(f"pos_y = {pos_y:_}") - output_lines.append(f"theta = {random_theta}") + output_lines.append(f"direction = {direction}") + if control_loop_library_path is not None: + output_lines.append( + f'control_loop_library_path = "{control_loop_library_path}"' + ) output_lines.append("") # Empty line for readability return "\n".join(output_lines) -def generate_dotbot_list(): - # Generate - toml_string = generate_lattice_toml(WIDTH_NODES, HEIGHT_NODES, SEP_X, SEP_Y) +@click.command() +@click.argument( + "output_path", + type=click.Path(dir_okay=False, writable=True), + default="init_state.toml", +) +@click.option( + "--width", + type=int, + default=WIDTH_NODES_DEFAULT, + help=f"Number of robots per row. Defaults to {WIDTH_NODES_DEFAULT}", +) +@click.option( + "--height", + type=int, + default=HEIGHT_NODES_DEFAULT, + help=f"Number of rows. Defaults to {HEIGHT_NODES_DEFAULT}", +) +@click.option( + "--start-x", + type=int, + default=START_X_DEFAULT, + help=f"Starting X position in mm. Defaults to {START_X_DEFAULT}", +) +@click.option( + "--start-y", + type=int, + default=START_Y_DEFAULT, + help=f"Starting Y position in mm. Defaults to {START_Y_DEFAULT}", +) +@click.option( + "--sep-x", + type=int, + default=SEP_X_DEFAULT, + help=f"Separation in mm between columns. Defaults to {SEP_X_DEFAULT}", +) +@click.option( + "--sep-y", + type=int, + default=SEP_Y_DEFAULT, + help=f"Separation in mm between rows. Defaults to {SEP_Y_DEFAULT}", +) +@click.option( + "--direction", + type=int, + default=None, + help="Default robot direction. Defaults to random generated direction", +) +@click.option( + "--control-loop-library-path", + type=click.Path(exists=True, dir_okay=False), + help="Path to an optional .so control loop library.", +) +def main( + output_path, + width, + height, + start_x, + start_y, + sep_x, + sep_y, + direction, + control_loop_library_path, +): + print(f"\nGenerating configuration with {width * height} robots.\n") + print(f" - Layout (row x col) : {height} x {width}") + print(f" - Start coordinate (X,Y) : {start_x}, {start_y}") + print(f" - Separation (X,Y) : {sep_x}, {sep_y}\n") + + toml_string = generate_lattice_toml( + width, + height, + start_x, + start_y, + sep_x, + sep_y, + direction, + control_loop_library_path, + ) # Save to file - output_path = Path(__file__).resolve().parent / "init_state.toml" with open(output_path, "w") as f: f.write(toml_string) @@ -56,4 +143,4 @@ def generate_dotbot_list(): if __name__ == "__main__": - generate_dotbot_list() + main() diff --git a/dotbot/examples/minimum_naming_game/init_state.toml b/dotbot/examples/minimum_naming_game/init_state.toml index aa46b34..2b8b720 100644 --- a/dotbot/examples/minimum_naming_game/init_state.toml +++ b/dotbot/examples/minimum_naming_game/init_state.toml @@ -3,172 +3,172 @@ address = "AAAAAAAA00000001" calibrated = 0xff pos_x = 120 pos_y = 120 -theta = 4.79 +direction = 26 [[dotbots]] address = "AAAAAAAA00000002" calibrated = 0xff pos_x = 360 pos_y = 120 -theta = 5.85 +direction = 26 [[dotbots]] address = "AAAAAAAA00000003" calibrated = 0xff pos_x = 600 pos_y = 120 -theta = 4.34 +direction = 26 [[dotbots]] address = "AAAAAAAA00000004" calibrated = 0xff pos_x = 840 pos_y = 120 -theta = 2.86 +direction = 26 [[dotbots]] address = "AAAAAAAA00000005" calibrated = 0xff pos_x = 1_080 pos_y = 120 -theta = 4.62 +direction = 26 [[dotbots]] address = "AAAAAAAA00000006" calibrated = 0xff pos_x = 120 pos_y = 360 -theta = 2.88 +direction = 26 [[dotbots]] address = "AAAAAAAA00000007" calibrated = 0xff pos_x = 360 pos_y = 360 -theta = 0.84 +direction = 26 [[dotbots]] address = "AAAAAAAA00000008" calibrated = 0xff pos_x = 600 pos_y = 360 -theta = 4.11 +direction = 26 [[dotbots]] address = "AAAAAAAA00000009" calibrated = 0xff pos_x = 840 pos_y = 360 -theta = 1.36 +direction = 26 [[dotbots]] address = "AAAAAAAA0000000A" calibrated = 0xff pos_x = 1_080 pos_y = 360 -theta = 1.7 +direction = 26 [[dotbots]] address = "AAAAAAAA0000000B" calibrated = 0xff pos_x = 120 pos_y = 600 -theta = 1.79 +direction = 26 [[dotbots]] address = "AAAAAAAA0000000C" calibrated = 0xff pos_x = 360 pos_y = 600 -theta = 5.67 +direction = 26 [[dotbots]] address = "AAAAAAAA0000000D" calibrated = 0xff pos_x = 600 pos_y = 600 -theta = 5.6 +direction = 26 [[dotbots]] address = "AAAAAAAA0000000E" calibrated = 0xff pos_x = 840 pos_y = 600 -theta = 3.07 +direction = 26 [[dotbots]] address = "AAAAAAAA0000000F" calibrated = 0xff pos_x = 1_080 pos_y = 600 -theta = 3.97 +direction = 26 [[dotbots]] address = "AAAAAAAA00000010" calibrated = 0xff pos_x = 120 pos_y = 840 -theta = 0.89 +direction = 26 [[dotbots]] address = "AAAAAAAA00000011" calibrated = 0xff pos_x = 360 pos_y = 840 -theta = 5.04 +direction = 26 [[dotbots]] address = "AAAAAAAA00000012" calibrated = 0xff pos_x = 600 pos_y = 840 -theta = 1.56 +direction = 26 [[dotbots]] address = "AAAAAAAA00000013" calibrated = 0xff pos_x = 840 pos_y = 840 -theta = 2.1 +direction = 26 [[dotbots]] address = "AAAAAAAA00000014" calibrated = 0xff pos_x = 1_080 pos_y = 840 -theta = 2.5 +direction = 26 [[dotbots]] address = "AAAAAAAA00000015" calibrated = 0xff pos_x = 120 pos_y = 1_080 -theta = 0.63 +direction = 26 [[dotbots]] address = "AAAAAAAA00000016" calibrated = 0xff pos_x = 360 pos_y = 1_080 -theta = 6.22 +direction = 26 [[dotbots]] address = "AAAAAAAA00000017" calibrated = 0xff pos_x = 600 pos_y = 1_080 -theta = 3.68 +direction = 26 [[dotbots]] address = "AAAAAAAA00000018" calibrated = 0xff pos_x = 840 pos_y = 1_080 -theta = 3.07 +direction = 26 [[dotbots]] address = "AAAAAAAA00000019" calibrated = 0xff pos_x = 1_080 pos_y = 1_080 -theta = 3.32 +direction = 26 diff --git a/dotbot/examples/minimum_naming_game/init_state_with_motion.toml b/dotbot/examples/minimum_naming_game/init_state_with_motion.toml index 5837d93..b923dca 100644 --- a/dotbot/examples/minimum_naming_game/init_state_with_motion.toml +++ b/dotbot/examples/minimum_naming_game/init_state_with_motion.toml @@ -1,62 +1,62 @@ [[dotbots]] address = "AAAAAAAA00000001" calibrated = 0xff -pos_x = 240 -pos_y = 240 -theta = 3.5 +pos_x = 300 +pos_y = 300 +direction = 180 [[dotbots]] address = "AAAAAAAA00000002" calibrated = 0xff -pos_x = 720 -pos_y = 240 -theta = 6.0 +pos_x = 600 +pos_y = 300 +direction = 180 [[dotbots]] address = "AAAAAAAA00000003" calibrated = 0xff -pos_x = 1_200 -pos_y = 240 -theta = 1.64 +pos_x = 900 +pos_y = 300 +direction = 180 [[dotbots]] address = "AAAAAAAA00000004" calibrated = 0xff -pos_x = 240 -pos_y = 720 -theta = 1.23 +pos_x = 300 +pos_y = 600 +direction = 180 [[dotbots]] address = "AAAAAAAA00000005" calibrated = 0xff -pos_x = 720 -pos_y = 720 -theta = 4.16 +pos_x = 600 +pos_y = 600 +direction = 180 [[dotbots]] address = "AAAAAAAA00000006" calibrated = 0xff -pos_x = 1_200 -pos_y = 720 -theta = 3.81 +pos_x = 900 +pos_y = 600 +direction = 180 [[dotbots]] address = "AAAAAAAA00000007" calibrated = 0xff -pos_x = 240 -pos_y = 1_200 -theta = 2.8 +pos_x = 300 +pos_y = 900 +direction = 180 [[dotbots]] address = "AAAAAAAA00000008" calibrated = 0xff -pos_x = 720 -pos_y = 1_200 -theta = 1.54 +pos_x = 600 +pos_y = 900 +direction = 180 [[dotbots]] address = "AAAAAAAA00000009" calibrated = 0xff -pos_x = 1_200 -pos_y = 1_200 -theta = 6.03 +pos_x = 900 +pos_y = 900 +direction = 180 From 89a3eec92611708ba6017dfb68a489ab41972fad Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Fri, 20 Mar 2026 15:07:19 +0100 Subject: [PATCH 14/17] dotbot/examples/work_and_charge: rework init pose generator script --- .../examples/work_and_charge/gen_init_pose.py | 24 +++++++++++++++---- .../examples/work_and_charge/init_state.toml | 16 ++++++------- 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/dotbot/examples/work_and_charge/gen_init_pose.py b/dotbot/examples/work_and_charge/gen_init_pose.py index 0576e36..23ce908 100644 --- a/dotbot/examples/work_and_charge/gen_init_pose.py +++ b/dotbot/examples/work_and_charge/gen_init_pose.py @@ -1,5 +1,7 @@ from pathlib import Path +import click + # Configuration Constants NUM_ROBOTS = 8 # Total robots to generate START_ID = 1 # Start at AAAAAAAA00000001 @@ -7,10 +9,21 @@ X_LEFT = 100 START_Y = 200 Y_STEP = 200 -THETA = 3.14 +DIRECTION = 180 -def generate_dotbot_script(): +@click.command() +@click.argument( + "output_path", + type=click.Path(dir_okay=False, writable=True), + default="init_state.toml", +) +@click.option( + "--control-loop-library-path", + type=click.Path(exists=True, dir_okay=False), + help="Path to an optional .so control loop library.", +) +def main(output_path, control_loop_library_path): lines = [] @@ -35,12 +48,13 @@ def generate_dotbot_script(): f"calibrated = 0xff\n" f"pos_x = {pos_x}\n" f"pos_y = {pos_y}\n" - f"theta = {THETA}\n" + f"direction = {DIRECTION}\n" ) + if control_loop_library_path is not None: + block += f'control_loop_library_path = "{control_loop_library_path}"\n' lines.append(block) # Save to file - output_path = Path(__file__).resolve().parent / "init_state.toml" with open(output_path, "w") as f: f.write("\n".join(lines)) @@ -48,4 +62,4 @@ def generate_dotbot_script(): if __name__ == "__main__": - generate_dotbot_script() + main() diff --git a/dotbot/examples/work_and_charge/init_state.toml b/dotbot/examples/work_and_charge/init_state.toml index 0e70945..5ae98e6 100644 --- a/dotbot/examples/work_and_charge/init_state.toml +++ b/dotbot/examples/work_and_charge/init_state.toml @@ -3,53 +3,53 @@ address = "AAAAAAAA00000001" calibrated = 0xff pos_x = 800 pos_y = 200 -theta = 3.14 +direction = 180 [[dotbots]] address = "AAAAAAAA00000002" calibrated = 0xff pos_x = 100 pos_y = 400 -theta = 3.14 +direction = 180 [[dotbots]] address = "AAAAAAAA00000003" calibrated = 0xff pos_x = 800 pos_y = 600 -theta = 3.14 +direction = 180 [[dotbots]] address = "AAAAAAAA00000004" calibrated = 0xff pos_x = 100 pos_y = 800 -theta = 3.14 +direction = 180 [[dotbots]] address = "AAAAAAAA00000005" calibrated = 0xff pos_x = 800 pos_y = 1_000 -theta = 3.14 +direction = 180 [[dotbots]] address = "AAAAAAAA00000006" calibrated = 0xff pos_x = 100 pos_y = 1_200 -theta = 3.14 +direction = 180 [[dotbots]] address = "AAAAAAAA00000007" calibrated = 0xff pos_x = 800 pos_y = 1_400 -theta = 3.14 +direction = 180 [[dotbots]] address = "AAAAAAAA00000008" calibrated = 0xff pos_x = 100 pos_y = 1_600 -theta = 3.14 +direction = 180 From 66566866cb98e4e7f7f685ad39723ae954098f9b Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Fri, 20 Mar 2026 15:07:58 +0100 Subject: [PATCH 15/17] dotbot/examples/charging_station: make init state configuration compatible with simulator --- .../charging_station_init_state.toml | 72 ++++++++++--------- 1 file changed, 40 insertions(+), 32 deletions(-) diff --git a/dotbot/examples/charging_station/charging_station_init_state.toml b/dotbot/examples/charging_station/charging_station_init_state.toml index 6ef74e1..d11d3ce 100644 --- a/dotbot/examples/charging_station/charging_station_init_state.toml +++ b/dotbot/examples/charging_station/charging_station_init_state.toml @@ -1,61 +1,69 @@ [[dotbots]] address = "BADCAFE111111111" # DotBot unique address -calibrated = 0x01 # optional, defaults to only first lighthouse calibrated -pos_x = 400_000 # [0, 1_000_000] -pos_y = 200_000 # [0, 1_000_000] -theta = 0.0 # [0.0, 2pi] +calibrated = 0xff # optional, defaults to all possible lighthouses calibrated +pos_x = 400 # [0, 1_000_000] +pos_y = 200 # [0, 1_000_000] +direction = 0 # [0, 360] [[dotbots]] address = "DEADBEEF22222222" -pos_x = 300_000 -pos_y = 100_000 -theta = 1.57 +calibrated = 0xff +pos_x = 300 +pos_y = 200 +direction = 90 [[dotbots]] address = "B0B0F00D33333333" -pos_x = 1_000_000 -pos_y = 1_000_000 -theta = 1.57 +calibrated = 0xff +pos_x = 1_000 +pos_y = 1_000 +direction = 90 [[dotbots]] address = "BADC0DE444444444" -pos_x = 500_000 -pos_y = 500_000 -theta = 3.14 +calibrated = 0xff +pos_x = 500 +pos_y = 500 +direction = 180 [[dotbots]] address = "5555555555555555" -pos_x = 10_000 -pos_y = 870_000 -theta = 3.14 +calibrated = 0xff +pos_x = 810 +pos_y = 870 +direction = 180 [[dotbots]] address = "6666666666666666" -pos_x = 280_000 -pos_y = 880_000 -theta = 3.14 +calibrated = 0xff +pos_x = 280 +pos_y = 880 +direction = 180 [[dotbots]] address = "7777777777777777" -pos_x = 120_000 -pos_y = 90_000 -theta = 3.14 +calibrated = 0xff +pos_x = 120 +pos_y = 990 +direction = 180 [[dotbots]] address = "8888888888888888" -pos_x = 800_000 -pos_y = 560_000 -theta = 3.14 +calibrated = 0xff +pos_x = 800 +pos_y = 560 +direction = 180 [[dotbots]] address = "9999999999999999" -pos_x = 990_000 -pos_y = 180_000 -theta = 1.14 - +calibrated = 0xff +pos_x = 990 +pos_y = 180 +direction = 65 [[dotbots]] address = "A000000000000000" -pos_x = 880_000 -pos_y = 80_000 -theta = 2.14 +calibrated = 0xff +pos_x = 900 +pos_y = 800 +direction = 123 From 470001c097099fed2ca8bf06418801800c2ffa6f Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Fri, 20 Mar 2026 16:39:59 +0100 Subject: [PATCH 16/17] dotbot/examples: fix lint --- dotbot/examples/minimum_naming_game/gen_init_pose.py | 2 -- dotbot/examples/work_and_charge/gen_init_pose.py | 1 - 2 files changed, 3 deletions(-) diff --git a/dotbot/examples/minimum_naming_game/gen_init_pose.py b/dotbot/examples/minimum_naming_game/gen_init_pose.py index 9418b22..ffbeb1a 100644 --- a/dotbot/examples/minimum_naming_game/gen_init_pose.py +++ b/dotbot/examples/minimum_naming_game/gen_init_pose.py @@ -1,6 +1,4 @@ -import math import random -from pathlib import Path import click from rich import print diff --git a/dotbot/examples/work_and_charge/gen_init_pose.py b/dotbot/examples/work_and_charge/gen_init_pose.py index 23ce908..1e6780d 100644 --- a/dotbot/examples/work_and_charge/gen_init_pose.py +++ b/dotbot/examples/work_and_charge/gen_init_pose.py @@ -1,4 +1,3 @@ -from pathlib import Path import click From ed3dde8363cea68478ee5fa5d152f927bce0ef71 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Wed, 25 Mar 2026 07:21:38 +0100 Subject: [PATCH 17/17] dotbot: add background map feature --- dotbot/controller.py | 1 + dotbot/controller_app.py | 13 +++ dotbot/examples/maps/labyrinth-2000x2000.png | Bin 0 -> 7042 bytes dotbot/examples/maps/labyrinth-2000x2000.svg | 115 +++++++++++++++++++ dotbot/frontend/src/DotBots.js | 3 +- dotbot/frontend/src/DotBotsMap.js | 4 + dotbot/frontend/src/RestApp.js | 15 ++- dotbot/frontend/src/utils/rest.js | 10 ++ dotbot/server.py | 14 +++ 9 files changed, 172 insertions(+), 3 deletions(-) create mode 100644 dotbot/examples/maps/labyrinth-2000x2000.png create mode 100644 dotbot/examples/maps/labyrinth-2000x2000.svg diff --git a/dotbot/controller.py b/dotbot/controller.py index a71d37f..c3f86b8 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -122,6 +122,7 @@ class ControllerSettings: network_id: str = NETWORK_ID_DEFAULT controller_http_port: int = CONTROLLER_HTTP_PORT_DEFAULT map_size: str = MAP_SIZE_DEFAULT + background_map: str = "" webbrowser: bool = False verbose: bool = False log_level: str = "info" diff --git a/dotbot/controller_app.py b/dotbot/controller_app.py index 39d5de3..fd7dbd3 100644 --- a/dotbot/controller_app.py +++ b/dotbot/controller_app.py @@ -120,6 +120,17 @@ type=str, help=f"Map size in mm. Defaults to '{MAP_SIZE_DEFAULT}'", ) +@click.option( + "-M", + "--background-map", + type=click.Path(exists=True, dir_okay=False), + help=( + f"Path to a background map image file in png format. The image should" + "be a top-down view of the environment, with 1024 pixels width and a " + "height proportional to the real map size. The map size should be set " + f"with the --map-size option (default: {MAP_SIZE_DEFAULT})." + ), +) def main( adapter, port, @@ -131,6 +142,7 @@ def main( network_id, controller_http_port, map_size, + background_map, webbrowser, verbose, log_level, @@ -153,6 +165,7 @@ def main( "network_id": network_id, "controller_http_port": controller_http_port, "map_size": map_size, + "background_map": background_map, "webbrowser": webbrowser, "verbose": verbose, "log_level": log_level, diff --git a/dotbot/examples/maps/labyrinth-2000x2000.png b/dotbot/examples/maps/labyrinth-2000x2000.png new file mode 100644 index 0000000000000000000000000000000000000000..9ec303f00b54a73ae1d4c13616115188955f22a4 GIT binary patch literal 7042 zcmeAS@N?(olHy`uVBq!ia0y~yU;#2&7&zE~RK2WrGmzpe@Q5sCU|{?Y!i-i1iZg+N zk|nMYCBgY=CFO}lsSM@i<$9TU*~Q6;1*v-ZMd`EO*+?-kNb`HTIEGZrd2@SX-<0dJ z?GKCFrgE20+haJf+hik4X~xc0iCoReTJKkRhS;%V{=R_C$snKQ<=yAo>>>ioHLEj-OJ1%vbH{N--F-wF{h)9vT$YYTdGZ(KV1_w)W`Z+!LyHh?Q`WAYW}ys zuB>^#T0MEqS>^_HkOu`6K%_$hh-6@70+B2nAaYa!6bhq3GMX5GsbDmJjFt?*WH4GL zf{Mh^GI1D}iRsny49inuduJERC1?J7{BiI4cQVPG+qOL0WqIvK+UvPHuboepyO+Dy z`DWGM4?pwm-_)|s>9V{Q|9v0hj~NUM*A4hzcW>OL&rsaEJ9*9S+588OJ+r!YVUOvt zjmJD@Jzj3u25b~wxGGV9?>F0nwf;;o;y|S4fbh$l{>%l9ytDo8RU(DZ8Z*PlXcW`VEog+r!X4qqruKF zDEh3UDRDFaTO?C03+8Xd!2Q3 zeoXC;`?J@YH@pUoyDB`nxY=AE*mbzkJ~)%~Fz)D$4(I?A;OKx3Fc606fR2IwKHK={ z{xybiXASkY-vib!4Z)j_?Xo_;W9!#*H?J`#_k8@%zsc;s0WbgxvYt&WvjNTiz}wRe z_it}x2G0jM{=3#3BLQR{XaFi;XkY}3GJ`V^8@TDq11={dz%xb$3Lr%Z4j_{18aN`Y VTl}|4a~}qo?&<31vd$@?2>|tjVLbo< literal 0 HcmV?d00001 diff --git a/dotbot/examples/maps/labyrinth-2000x2000.svg b/dotbot/examples/maps/labyrinth-2000x2000.svg new file mode 100644 index 0000000..aecc3e8 --- /dev/null +++ b/dotbot/examples/maps/labyrinth-2000x2000.svg @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + diff --git a/dotbot/frontend/src/DotBots.js b/dotbot/frontend/src/DotBots.js index 2ba35e6..f6dac25 100644 --- a/dotbot/frontend/src/DotBots.js +++ b/dotbot/frontend/src/DotBots.js @@ -12,7 +12,7 @@ import { ApplicationType, inactiveAddress, maxWaypoints, maxPositionHistory } fr import logger from './utils/logger'; const log = logger.child({module: 'DotBots'}); -const DotBots = ({ dotbots, areaSize, updateDotbots, publishCommand, publish }) => { +const DotBots = ({ dotbots, areaSize, backgroundMap, updateDotbots, publishCommand, publish }) => { const [ activeDotbot, setActiveDotbot ] = useState(inactiveAddress); const [ showDotBotHistory, setShowDotBotHistory ] = useState(true); const [ dotbotHistorySize, setDotbotHistorySize ] = useState(maxPositionHistory); @@ -243,6 +243,7 @@ const DotBots = ({ dotbots, areaSize, updateDotbots, publishCommand, publish }) mapClicked={mapClicked} mapSize={1000} areaSize={areaSize} + backgroundMap={backgroundMap} publish={publish} /> diff --git a/dotbot/frontend/src/DotBotsMap.js b/dotbot/frontend/src/DotBotsMap.js index f80da0b..a21efc9 100644 --- a/dotbot/frontend/src/DotBotsMap.js +++ b/dotbot/frontend/src/DotBotsMap.js @@ -194,6 +194,10 @@ export const DotBotsMap = (props) => { + {/* Background map */} + {props.backgroundMap && + + } {/* Map grid */} mapClicked(event)}/> {/* DotBots points */} diff --git a/dotbot/frontend/src/RestApp.js b/dotbot/frontend/src/RestApp.js index d88f69a..eb7ac8f 100644 --- a/dotbot/frontend/src/RestApp.js +++ b/dotbot/frontend/src/RestApp.js @@ -4,7 +4,7 @@ import { NotificationType } from "./utils/constants"; import { handleDotBotUpdate } from "./utils/helpers"; import useWebSocket from 'react-use-websocket'; -import { apiFetchDotbots, apiFetchMapSize, apiUpdateMoveRaw, apiUpdateRgbLed, apiUpdateWaypoints, apiClearPositionsHistory } from "./utils/rest"; +import { apiFetchDotbots, apiFetchMapSize, apiFetchBackgroundMap, apiUpdateMoveRaw, apiUpdateRgbLed, apiUpdateWaypoints, apiClearPositionsHistory } from "./utils/rest"; import DotBots from './DotBots'; import logger from './utils/logger'; @@ -12,6 +12,7 @@ const log = logger.child({module: 'RestApp'}); const RestApp = () => { const [areaSize, setAreaSize] = useState(undefined); + const [backgroundMap, setBackgroundMap] = useState(undefined); const [dotbots, setDotbots] = useState([]); const websocketUrl = `ws://localhost:8000/controller/ws/status`; @@ -28,6 +29,12 @@ const RestApp = () => { }, [setAreaSize] ); + const fetchBackgroundMap = useCallback(async () => { + const data = await apiFetchBackgroundMap().catch(error => console.log(error)); + setBackgroundMap(data); + }, [setBackgroundMap] + ); + const publishCommand = async (address, application, command, data) => { if (command === "move_raw") { return await apiUpdateMoveRaw(address, application, data.left_x, data.left_y, data.right_x, data.right_y).catch(error => console.log(error)); @@ -79,7 +86,10 @@ const RestApp = () => { if (!areaSize) { fetchAreaSize(); } - }, [dotbots, areaSize, fetchDotBots, fetchAreaSize] + if (!backgroundMap) { + fetchBackgroundMap(); + } + }, [dotbots, areaSize, backgroundMap, fetchDotBots, fetchAreaSize, fetchBackgroundMap] ); return ( @@ -89,6 +99,7 @@ const RestApp = () => { { ).then(res => res.data); } +export const apiFetchBackgroundMap = async () => { + log.info("Fetching background map from API");; + return await axios.get( + `${API_URL}/controller/background_map`, + ).then(res => { + console.log(`Received background map data from API ${res.data.slice(0,100)} characters long`); + return res.data + }); +} + export const apiUpdateMoveRaw = async (address, application, left_x, left_y, right_x, right_y) => { log.info(`Setting move raw for dotbot ${address} with values (${left_x}, ${left_y}, ${right_x}, ${right_y})`); const command = { left_x: parseInt(left_x), left_y: parseInt(left_y), right_x: parseInt(right_x), right_y: parseInt(right_y) }; diff --git a/dotbot/server.py b/dotbot/server.py index d677a7c..d2f417d 100644 --- a/dotbot/server.py +++ b/dotbot/server.py @@ -5,6 +5,7 @@ """Module for the web server application.""" +import base64 import os from typing import Annotated, List @@ -288,6 +289,19 @@ async def map_size(): return api.controller.map_size +@api.get( + path="/controller/background_map", + response_model=str, + summary="Return the background map of the controller", + tags=["controller"], +) +async def background_map(): + """Background map HTTP GET handler.""" + with open(api.controller.settings.background_map, "rb") as f: + encoded_string = base64.b64encode(f.read()).decode("utf-8") + return encoded_string + + @api.websocket("/controller/ws/status") async def websocket_endpoint(websocket: WebSocket): """Websocket server endpoint."""