refactor: separate odometry and implement pure pursuit

bricks/_common/common.mk

@@ -307,6 +307,7 @@ PY_EXTRA_SRC_C += $(addprefix bricks/_common/,\
 # --- UNIFIED ODOMETRY ENGINE (3-LAYER ARCHITECTURE) ---
 # Compiling both the module wrapper and the logic core
 PY_EXTRA_SRC_C += pybricks/experimental/odometry.c
+PY_EXTRA_SRC_C += pybricks/experimental/pursuit.c
 
 # Not all MCUs support thumb2 instructions.
 ifeq ($(PB_MCU_FAMILY),native)

pybricks/experimental/micropyhon.mk

@@ -1,4 +1,4 @@
 # Always compile the shared Python wrapper
 PYBRICKS_SRC_C += pybricks/experimental/pb_module_experimental.c
 PYBRICKS_SRC_C += pybricks/experimental/odometry.c
-
+PYBRICKS_SRC_C += pybricks/experimental/pursuit.c

pybricks/experimental/odometry.c

@@ -14,227 +14,114 @@
 #include <pbio/control.h>
 #include "pybricks/experimental/odometry.h"
 #include "pybricks/experimental/platform_math.h"
-
 #include <pbio/imu.h>
 
-// Hardware Object structure declaration
 typedef struct _pb_type_pupdevices_Motor_obj_t {
     mp_obj_base_t base;
-    pbio_servo_t *srv;
+    pbio_servo_t *servo;
 } pb_type_pupdevices_Motor_obj_t;
 
-// State Variables
-static pbio_servo_t *left_servo_ptr = NULL;
-static pbio_servo_t *right_servo_ptr = NULL;
-
-volatile bool odom_running = false;
-volatile uint32_t last_odom_time_ms = 0;
-volatile uint32_t last_pursuit_time_ms = 0;
-
-volatile uint32_t fps = 200;
-volatile uint32_t mstowait = 5;
-volatile float global_x = 0.0f, global_y = 0.0f, global_h = 0.0f;
-volatile int32_t last_left_angle = 0, last_right_angle = 0;
-
-// Store the last IMU read to calculate deltas
-volatile float last_imu_heading = 0.0f;
-
-float odom_deg_to_mm = 1.0f;
-float odom_inv_track = 1.0f;
-
-volatile bool pursuit_running = false;
-volatile float p_target_speed = 0.0f;
-volatile float p_lookahead = 120.0f;
-volatile float sp_a = 0.0f, sp_b = 0.0f, sp_c = 0.0f, sp_d = 0.0f, sp_x_end = 0.0f;
-
-// FPS Counter Variables
-volatile uint32_t vm_loop_counter = 0;
-volatile uint32_t current_fps = 0;
-volatile uint32_t last_fps_time_ms = 0;
+pb_odom_state_t odom_state = {0};
 
 void pb_background_odometry_update(void) {
-    if (!odom_running) {
+    if (!odom_state.running) {
         return;
     }
+    uint32_t current_time_ms = mp_hal_ticks_ms();
 
-    uint32_t now = mp_hal_ticks_ms();
-
-    // Every 1000ms, save the count and reset
-    if (now - last_fps_time_ms >= 1000) {
-        current_fps = vm_loop_counter;
-        vm_loop_counter = 0;
-        last_fps_time_ms = now;
+    if (current_time_ms - odom_state.last_fps_time_ms >= 1000) {
+        odom_state.current_fps = odom_state.vm_loop_counter;
+        odom_state.vm_loop_counter = 0;
+        odom_state.last_fps_time_ms = current_time_ms;
     }
-    // -------------------------
 
-    if (!left_servo_ptr || !right_servo_ptr) {
+    if (!odom_state.left_servo || !odom_state.right_servo) {
         return;
     }
 
-    // --- RATE CAP: 200 Hz (5ms) ---
-    if (now - last_odom_time_ms < mstowait) {
+    if (current_time_ms - odom_state.last_time_ms < odom_state.mstowait) {
         return;
     }
-    last_odom_time_ms = now;
-    // ------------------------------
-    vm_loop_counter++;
+    odom_state.last_time_ms = current_time_ms;
+    odom_state.vm_loop_counter++;
 
     int32_t cur_l, cur_r, unused_rate;
-    pbio_servo_get_state_user(left_servo_ptr, &cur_l, &unused_rate);
-    pbio_servo_get_state_user(right_servo_ptr, &cur_r, &unused_rate);
-
-    int32_t delta_l = cur_l - last_left_angle;
-    int32_t delta_r = cur_r - last_right_angle;
+    pbio_servo_get_state_user(odom_state.left_servo, &cur_l, &unused_rate);
+    pbio_servo_get_state_user(odom_state.right_servo, &cur_r, &unused_rate);
 
-    // Update encoder state immediately
-    last_left_angle = cur_l;
-    last_right_angle = cur_r;
+    int32_t delta_l = cur_l - odom_state.last_left_angle;
+    int32_t delta_r = cur_r - odom_state.last_right_angle;
+    odom_state.last_left_angle = cur_l;
+    odom_state.last_right_angle = cur_r;
 
-    // Calculate Heading Delta directly from the IMU (axis 0 = yaw)
-    float current_imu_heading = pbio_imu_get_heading(0);
-    float dH = current_imu_heading - last_imu_heading;
-    last_imu_heading = current_imu_heading;
+    float current_heading = pbio_imu_get_heading(0);
+    float delta_h = current_heading - odom_state.last_imu_heading;
+    odom_state.last_imu_heading = current_heading;
 
-    // Protect against the IMU wrapping across the -PI/PI boundary
-    while (dH > 3.14159f) {
-        dH -= 6.28318f;
+    while (delta_h > 3.14159f) {
+        delta_h -= 6.28318f;
     }
-    while (dH < -3.14159f) {
-        dH += 6.28318f;
+    while (delta_h < -3.14159f) {
+        delta_h += 6.28318f;
     }
 
-    // Calculate the average heading for this tick's coordinate projection
-    float avg_h = global_h + (dH * 0.5f);
+    float avg_heading = odom_state.global_h + (delta_h * 0.5f);
+    odom_state.global_h += delta_h;
 
-    // Accumulate the global heading and wrap it safely
-    global_h += dH;
-    while (global_h > 3.14159f) {
-        global_h -= 6.28318f;
+    while (odom_state.global_h > 3.14159f) {
+        odom_state.global_h -= 6.28318f;
     }
-    while (global_h < -3.14159f) {
-        global_h += 6.28318f;
+    while (odom_state.global_h < -3.14159f) {
+        odom_state.global_h += 6.28318f;
     }
 
-    // Project X/Y coordinates if the wheels actually moved
     if (delta_l != 0 || delta_r != 0) {
-        float dL = (float)delta_l * odom_deg_to_mm;
-        float dR = (float)delta_r * odom_deg_to_mm;
+        float dL = (float)delta_l * odom_state.deg_to_mm;
+        float dR = (float)delta_r * odom_state.deg_to_mm;
         float dD = (dR + dL) * 0.5f;
 
-        global_x += dD * pb_fast_cos(avg_h);
-        global_y += dD * pb_fast_sin(avg_h);
+        odom_state.global_x += dD * pb_fast_cos(avg_heading);
+        odom_state.global_y += dD * pb_fast_sin(avg_heading);
     }
 }
 
-void pb_background_pursuit_update(void) {
-    if (!pursuit_running || !left_servo_ptr || !right_servo_ptr) {
-        return;
-    }
-
-    // --- RATE CAP: 100 Hz (10ms) ---
-    uint32_t now = mp_hal_ticks_ms();
-    if (now - last_pursuit_time_ms < mstowait) {
-        return;
-    }
-    last_pursuit_time_ms = now;
-    // -------------------------------
-
-    if (global_x >= sp_x_end) {
-        pursuit_running = false;
-        pbio_servo_stop(left_servo_ptr, PBIO_CONTROL_ON_COMPLETION_BRAKE);
-        pbio_servo_stop(right_servo_ptr, PBIO_CONTROL_ON_COMPLETION_BRAKE);
-        return;
-    }
-
-    float target_x = global_x + p_lookahead;
-    if (target_x > sp_x_end) {
-        target_x = sp_x_end;
-    }
-
-    float target_y = (sp_a * (target_x * target_x * target_x)) +
-        (sp_b * (target_x * target_x)) +
-        (sp_c * target_x) + sp_d;
-
-    float x_dif = target_x - global_x;
-    float y_dif = target_y - global_y;
-    float relative_y = (y_dif * pb_fast_cos(global_h)) - (x_dif * pb_fast_sin(global_h));
-    float dist_sq = (x_dif * x_dif) + (y_dif * y_dif);
-
-    float m_left = 1.0f, m_right = 1.0f;
-    if (relative_y > 0.001f || relative_y < -0.001f) {
-        float radius = -(dist_sq / (2.0f * relative_y));
-        float track = 1.0f / odom_inv_track;
-        m_right = (2.0f * radius) / ((2.0f * radius) + track);
-        m_left = (2.0f * radius) / ((2.0f * radius) - track);
-    }
-
-    pbio_servo_run_forever(left_servo_ptr, (int32_t)(p_target_speed * m_left));
-    pbio_servo_run_forever(right_servo_ptr, (int32_t)(p_target_speed * m_right));
-}
-
-// MicroPython Wrappers
 mp_obj_t experimental_start_odometry(size_t n_args, const mp_obj_t *args) {
-    left_servo_ptr = ((pb_type_pupdevices_Motor_obj_t *)MP_OBJ_TO_PTR(args[0]))->srv;
-    right_servo_ptr = ((pb_type_pupdevices_Motor_obj_t *)MP_OBJ_TO_PTR(args[1]))->srv;
-
-    // Using mm_per_deg directly as passed from Python
-    odom_deg_to_mm = mp_obj_get_float(args[2]);
-    odom_inv_track = 1.0f / mp_obj_get_float(args[3]);
+    odom_state.left_servo = ((pb_type_pupdevices_Motor_obj_t *)MP_OBJ_TO_PTR(args[0]))->servo;
+    odom_state.right_servo = ((pb_type_pupdevices_Motor_obj_t *)MP_OBJ_TO_PTR(args[1]))->servo;
 
-    global_x = mp_obj_get_float(args[4]);
-    global_y = mp_obj_get_float(args[5]);
-    global_h = mp_obj_get_float(args[6]);
-    fps = mp_obj_get_int(args[7]);
+    odom_state.deg_to_mm = mp_obj_get_float(args[2]);
+    odom_state.inv_track = 1.0f / mp_obj_get_float(args[3]);
+    odom_state.global_x = mp_obj_get_float(args[4]);
+    odom_state.global_y = mp_obj_get_float(args[5]);
+    odom_state.global_h = mp_obj_get_float(args[6]);
+    uint32_t fps = mp_obj_get_int(args[7]);
+    odom_state.mstowait = 1000 / fps;
 
-    mstowait = 1000 / fps;
     int32_t unused;
-    pbio_servo_get_state_user(left_servo_ptr, (int32_t *)&last_left_angle, &unused);
-    pbio_servo_get_state_user(right_servo_ptr, (int32_t *)&last_right_angle, &unused);
+    pbio_servo_get_state_user(odom_state.left_servo, (int32_t *)&odom_state.last_left_angle, &unused);
+    pbio_servo_get_state_user(odom_state.right_servo, (int32_t *)&odom_state.last_right_angle, &unused);
 
-    // Sync the IMU baseline before the loop starts to prevent massive delta jumps
-    last_imu_heading = pbio_imu_get_heading(0);
-
-    odom_running = true;
+    odom_state.last_imu_heading = pbio_imu_get_heading(0);
+    odom_state.running = true;
     return mp_const_none;
 }
 
 mp_obj_t experimental_get_odometry(void) {
     mp_obj_t tuple[3] = {
-        mp_obj_new_float_from_f(global_x),
-        mp_obj_new_float_from_f(global_y),
-        mp_obj_new_float_from_f(global_h)
+        mp_obj_new_float_from_f(odom_state.global_x),
+        mp_obj_new_float_from_f(odom_state.global_y),
+        mp_obj_new_float_from_f(odom_state.global_h)
     };
     return mp_obj_new_tuple(3, tuple);
 }
 
-mp_obj_t experimental_start_pursuit(size_t n_args, const mp_obj_t *args) {
-    sp_a = mp_obj_get_float(args[0]);
-    sp_b = mp_obj_get_float(args[1]);
-    sp_c = mp_obj_get_float(args[2]);
-    sp_d = mp_obj_get_float(args[3]);
-    sp_x_end = mp_obj_get_float(args[4]);
-    p_target_speed = mp_obj_get_float(args[5]);
-    p_lookahead = mp_obj_get_float(args[6]);
-    pursuit_running = true;
-    return mp_const_none;
-}
-
-mp_obj_t experimental_stop_pursuit(void) {
-    pursuit_running = false;
-    if (left_servo_ptr && right_servo_ptr) {
-        pbio_servo_stop(left_servo_ptr, PBIO_CONTROL_ON_COMPLETION_BRAKE);
-        pbio_servo_stop(right_servo_ptr, PBIO_CONTROL_ON_COMPLETION_BRAKE);
-    }
-    return mp_const_none;
-}
-
 mp_obj_t experimental_stop_odometry(void) {
-    odom_running = false;
+    odom_state.running = false;
     return mp_const_none;
 }
 
 mp_obj_t experimental_get_fps(void) {
-    return mp_obj_new_int_from_uint(current_fps);
+    return mp_obj_new_int_from_uint(odom_state.current_fps);
 }
 
 #endif // PYBRICKS_PY_EXPERIMENTAL

pybricks/experimental/odometry.h

@@ -2,9 +2,39 @@
 #define PYBRICKS_EXPERIMENTAL_ODOMETRY_H
 
 #include "py/obj.h"
+#include <pbio/servo.h>
+#include <stdbool.h>
+#include <stdint.h>
 
-// Background update hooks
+// Optimized for Cortex-M4F Alignment
+typedef struct {
+    pbio_servo_t *left_servo;
+    pbio_servo_t *right_servo;
+
+    volatile uint32_t last_time_ms;
+    volatile uint32_t mstowait;
+
+    volatile float global_x;
+    volatile float global_y;
+    volatile float global_h;
+
+    volatile int32_t last_left_angle;
+    volatile int32_t last_right_angle;
+    volatile float last_imu_heading;
+
+    float deg_to_mm;
+    float inv_track;
+
+    volatile uint32_t current_fps;
+    volatile uint32_t vm_loop_counter;
+    volatile uint32_t last_fps_time_ms;
+
+    volatile bool running;
+} pb_odom_state_t;
+
+extern pb_odom_state_t odom_state;
+
+// Background update hook
 void pb_background_odometry_update(void);
-void pb_background_pursuit_update(void);
 
-#endif
+#endif // PYBRICKS_EXPERIMENTAL_ODOMETRY_H

pybricks/experimental/pb_module_experimental.c

@@ -18,8 +18,8 @@ static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_start_odometry_obj, 8, 8
 static MP_DEFINE_CONST_FUN_OBJ_0(experimental_get_odometry_obj, experimental_get_odometry);
 static MP_DEFINE_CONST_FUN_OBJ_0(experimental_stop_odometry_obj, experimental_stop_odometry);
 
-// FIXED: Pursuit takes 7 arguments (a, b, c, d, x_end, speed, lookahead), not 4.
-static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_start_pursuit_obj, 7, 7, experimental_start_pursuit);
+// Pursuit now takes 3 list arguments: (spline_coefficients, drive_base, tuning_variables)
+static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_start_pursuit_obj, 3, 3, experimental_start_pursuit);
 
 static MP_DEFINE_CONST_FUN_OBJ_0(experimental_stop_pursuit_obj, experimental_stop_pursuit);
 static MP_DEFINE_CONST_FUN_OBJ_0(experimental_get_fps_obj, experimental_get_fps);