diff --git a/.github/workflows/continuous-integration.yml b/.github/workflows/continuous-integration.yml
index 43eef5b2..4d2786f3 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:
diff --git a/doc/conf.py b/doc/conf.py
index 88e624b6..52eb0e2f 100644
--- a/doc/conf.py
+++ b/doc/conf.py
@@ -46,6 +46,9 @@
     ("py:class", r"Query"),
     ("py:class", r"PydanticUndefined"),
     ("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 -------------------------------------------------
diff --git a/dotbot/dotbot_simulator.py b/dotbot/dotbot_simulator.py
index 3cfb1c7f..bacbad87 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
@@ -24,16 +26,17 @@
 from dotbot.protocol import PayloadDotBotAdvertisement, PayloadType
 
 Kv = 400  # motor speed constant in RPM
-r = 50  # motor reduction ratio
-R = 25  # wheel radius 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 = 1.2
-REDUCE_SPEED_FACTOR = 0.7
+ANGULAR_SPEED_GAIN = 2
+REDUCE_SPEED_FACTOR = 0.8
 REDUCE_SPEED_ANGLE = 25
+DIRECTION_THRESHOLD = 50  # threshold to update the direction (50mm)
 
 SIMULATOR_STEP_DELTA_T = 0.02  # 20 ms
 
@@ -59,7 +62,11 @@ 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
-    return pwm * R * Kv / (r * 127)
+    if pwm > 100:
+        pwm = 100
+    if pwm < -100:
+        pwm = -100
+    return pwm * D * Kv / (R * 127)
 
 
 class DotBotSimulatorMode(Enum):
@@ -81,10 +88,26 @@ 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
+    custom_control_loop_library: Path = None
+
+
+class RobotControl(ctypes.Structure):
+    _fields_ = [
+        ("pos_x", ctypes.c_uint32),
+        ("pos_y", ctypes.c_uint32),
+        ("direction", ctypes.c_int16),
+        ("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):
@@ -103,13 +126,16 @@ 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
+        self._control_loop_func = self._init_control_loop()
         self.time_elapsed_s = 0
 
         self.pwm_left = 0
         self.pwm_right = 0
+        self.direction = settings.direction
 
         self.calibrated = settings.calibrated
         self.waypoint_threshold = 0
@@ -130,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):
@@ -144,39 +177,34 @@ 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) * (
             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)
+        dx = x_dot * SIMULATOR_STEP_DELTA_T
+        dy = 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
 
-        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)
+        if sqrt(dx**2 + dy**2):
+            self.direction = int(-1 * atan2(dx, dy) * 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),
+            direction=int(self.direction),
+            pwm_left=int(self.pwm_left),
+            pwm_right=int(self.pwm_right),
         )
         self.time_elapsed_s += SIMULATOR_STEP_DELTA_T
 
@@ -189,12 +217,63 @@ def update_state(self):
             if is_stopped:
                 break
 
-    def _compute_automatic_control(self):
-        if self.controller_mode != DotBotSimulatorMode.AUTOMATIC:
-            return
+    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."""
+        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.direction
+        self.custom_robot_control.waypoints_length = 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
+        )
+        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
+
+        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
 
-        delta_x = self.pos_x - self.waypoints[self.waypoint_index].pos_x
-        delta_y = self.pos_y - self.waypoints[self.waypoint_index].pos_y
+        self.logger.info(
+            "Custom loop",
+            pwm_left=self.pwm_left,
+            pwm_right=self.pwm_right,
+            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,
+            waypoint_x=self.custom_robot_control.waypoint_x,
+            waypoint_y=self.custom_robot_control.waypoint_y,
+        )
+
+    def _control_loop_default(self):
+        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
@@ -210,46 +289,49 @@ def _compute_automatic_control(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.direction
+        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) * 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
 
         self.logger.info(
-            "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):
         """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
@@ -262,7 +344,7 @@ def advertise(self):
                 packet=Packet.from_payload(
                     PayloadDotBotAdvertisement(
                         calibrated=self.calibrated,
-                        direction=int(self.theta * 180 / pi + 90),
+                        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,
@@ -288,21 +370,16 @@ 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
-                        self.controller_mode = DotBotSimulatorMode.MANUAL
                         self.waypoint_threshold = frame.packet.payload.threshold
                         self.waypoints = frame.packet.payload.waypoints
                         self.logger.info(
@@ -312,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...")
diff --git a/dotbot/examples/charging_station/charging_station.py b/dotbot/examples/charging_station/charging_station.py
index dfc0f30a..2ba763f1 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)
diff --git a/dotbot/examples/charging_station/charging_station_init_state.toml b/dotbot/examples/charging_station/charging_station_init_state.toml
index 6ef74e1e..d11d3ce2 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
diff --git a/dotbot/examples/minimum_naming_game/gen_init_pose.py b/dotbot/examples/minimum_naming_game/gen_init_pose.py
index 1be4766e..ffbeb1aa 100644
--- a/dotbot/examples/minimum_naming_game/gen_init_pose.py
+++ b/dotbot/examples/minimum_naming_game/gen_init_pose.py
@@ -1,21 +1,26 @@
-import math
 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 +33,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 +43,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 +141,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 aa46b34f..2b8b720d 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 5837d93d..b923dca0 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
diff --git a/dotbot/examples/work_and_charge/gen_init_pose.py b/dotbot/examples/work_and_charge/gen_init_pose.py
index 0576e361..1e6780d0 100644
--- a/dotbot/examples/work_and_charge/gen_init_pose.py
+++ b/dotbot/examples/work_and_charge/gen_init_pose.py
@@ -1,4 +1,5 @@
-from pathlib import Path
+
+import click
 
 # Configuration Constants
 NUM_ROBOTS = 8  # Total robots to generate
@@ -7,10 +8,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 +47,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 +61,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 0e70945a..5ae98e63 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
diff --git a/dotbot/frontend/src/DotBotsMap.js b/dotbot/frontend/src/DotBotsMap.js
index 4b08bbc4..f80da0bf 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) => {
         <DotBotsPosition key={`position-${index}`} index={index} point={point} color={rgbColor} opacity={opacity} history={props.dotbot.position_history.slice(-props.historySize)} {...props} />
       ))
     )}
-    <g transform={`rotate(${rotation} ${posX} ${posY})`} stroke={`${(props.dotbot.address === props.active) ? "black" : "none"}`} strokeWidth="1">
     <circle cx={posX} cy={posY}
         r={radius}
         opacity={opacity}
         fill={rgbColor}
         style={{ cursor: "pointer" }}
+        stroke={`${(props.dotbot.address === props.active) ? "black" : "none"}`} strokeWidth="1"
         onClick={
           () => {
             props.updateActive(props.dotbot.address === props.active ? inactiveAddress : props.dotbot.address)
@@ -149,10 +149,11 @@ const DotBotsMapPoint = React.memo((props) => {
         onMouseLeave={onMouseLeave} >
       <title>{`${props.dotbot.address}@${posX}x${posY}`}</title>
     </circle>
-    {(props.dotbot.direction) &&
+    {(props.dotbot.direction !== -1000) &&
+    <g transform={`rotate(${rotation} ${posX} ${posY})`} stroke={`${(props.dotbot.address === props.active) ? "black" : "none"}`} strokeWidth="1">
       <polygon points={`${posX - radius + 10 * props.mapSize / props.areaSize.width},${posY + radius + directionShift} ${posX + radius - 10 * props.mapSize / props.areaSize.width},${posY + radius + directionShift} ${posX},${posY + radius + directionSize + directionShift}`} fill={rgbColor} opacity={opacity} />
-    }
     </g>
+    }
     </>
   )
 })
diff --git a/pyproject.toml b/pyproject.toml
index 57420ee6..652b0db2 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",
diff --git a/simulator_init_state.toml b/simulator_init_state.toml
index 6c09d66f..4869d438 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]
+direction = 0 # [0, 360] in degrees, 0 is facing north, increasing clockwise
 
 [[dotbots]]
 address = "DEADBEEF22222222"
 pos_x = 400
 pos_y = 200
-theta = 1.57
+direction = 180
 
 [[dotbots]]
 address = "B0B0F00D33333333"
 pos_x = 1_500
 pos_y = 1_500
-theta = 1.57
+direction = 180
 
 [[dotbots]]
 address = "BADC0DE444444444"
 pos_x = 1_000
 pos_y = 1_000
-theta = 3.14
+direction = 360
diff --git a/utils/control_loop/.gitignore b/utils/control_loop/.gitignore
new file mode 100644
index 00000000..567609b1
--- /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 00000000..3578431f
--- /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 00000000..7be1dbfa
--- /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=<path to DotBot-libs base directory> -DDOTBOT_VERSION=<DotBot 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.