From 9c5e7a729a038b0ee16da43a2073372e5706f998 Mon Sep 17 00:00:00 2001 From: techcatgato Date: Wed, 29 Jul 2026 18:04:49 +0300 Subject: [PATCH 1/2] refactor: separate odometry and implement pure pursuit --- .github/build-each-commit.py | 0 pybricks/experimental/micropyhon.mk | 2 +- pybricks/experimental/odometry.c | 219 ++++----------- pybricks/experimental/odometry.h | 36 ++- .../experimental/pb_module_experimental.c | 4 +- pybricks/experimental/pursuit.c | 258 ++++++++++++++++++ pybricks/experimental/pursuit.h | 40 +++ 7 files changed, 387 insertions(+), 172 deletions(-) mode change 100644 => 100755 .github/build-each-commit.py create mode 100644 pybricks/experimental/pursuit.c create mode 100644 pybricks/experimental/pursuit.h diff --git a/.github/build-each-commit.py b/.github/build-each-commit.py old mode 100644 new mode 100755 diff --git a/pybricks/experimental/micropyhon.mk b/pybricks/experimental/micropyhon.mk index 58c7ac5e1..c75b8f1e1 100644 --- a/pybricks/experimental/micropyhon.mk +++ b/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 \ No newline at end of file diff --git a/pybricks/experimental/odometry.c b/pybricks/experimental/odometry.c index f80882c37..f57672495 100644 --- a/pybricks/experimental/odometry.c +++ b/pybricks/experimental/odometry.c @@ -14,227 +14,114 @@ #include #include "pybricks/experimental/odometry.h" #include "pybricks/experimental/platform_math.h" - #include -// 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 diff --git a/pybricks/experimental/odometry.h b/pybricks/experimental/odometry.h index 70f06d353..6358fbe1d 100644 --- a/pybricks/experimental/odometry.h +++ b/pybricks/experimental/odometry.h @@ -2,9 +2,39 @@ #define PYBRICKS_EXPERIMENTAL_ODOMETRY_H #include "py/obj.h" +#include +#include +#include -// 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 diff --git a/pybricks/experimental/pb_module_experimental.c b/pybricks/experimental/pb_module_experimental.c index 474e0f258..035f6634d 100644 --- a/pybricks/experimental/pb_module_experimental.c +++ b/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); diff --git a/pybricks/experimental/pursuit.c b/pybricks/experimental/pursuit.c new file mode 100644 index 000000000..ff6fb02a0 --- /dev/null +++ b/pybricks/experimental/pursuit.c @@ -0,0 +1,258 @@ +// SPDX-License-Identifier: MIT +#include "py/mpconfig.h" + +#if PYBRICKS_PY_EXPERIMENTAL + +#include "py/mphal.h" +#include "py/runtime.h" +#include +#include +#include + +#include +#include +#include "pybricks/experimental/odometry.h" +#include "pybricks/experimental/pursuit.h" +#include "pybricks/experimental/platform_math.h" + +pb_pursuit_state_t pursuit_state = {0}; + +static float evaluate_x(float tau, uint8_t derivative) { + uint16_t spline_count; + if (tau != pursuit_state.total_splines) { + spline_count = (uint16_t)floorf(tau); + } else { + spline_count = pursuit_state.total_splines - 1; + } + + float t = tau - (float)spline_count; + float ax = pursuit_state.spline_coefficients[spline_count][0]; + float bx = pursuit_state.spline_coefficients[spline_count][1]; + float cx = pursuit_state.spline_coefficients[spline_count][2]; + float dx = pursuit_state.spline_coefficients[spline_count][3]; + + if (derivative == 0) { + return ax * t * t * t + bx * t * t + cx * t + dx; + } + if (derivative == 1) { + return 3.0f * ax * t * t + 2.0f * bx * t + cx; + } + if (derivative == 2) { + return 6.0f * ax * t + 2.0f * bx; + } + return 0.0f; +} + +static float evaluate_y(float tau, uint8_t derivative) { + uint16_t spline_count; + if (tau != pursuit_state.total_splines) { + spline_count = (uint16_t)floorf(tau); + } else { + spline_count = pursuit_state.total_splines - 1; + } + + float t = tau - (float)spline_count; + float ay = pursuit_state.spline_coefficients[spline_count][4]; + float by = pursuit_state.spline_coefficients[spline_count][5]; + float cy = pursuit_state.spline_coefficients[spline_count][6]; + float dy = pursuit_state.spline_coefficients[spline_count][7]; + + if (derivative == 0) { + return ay * t * t * t + by * t * t + cy * t + dy; + } + if (derivative == 1) { + return 3.0f * ay * t * t + 2.0f * by * t + cy; + } + if (derivative == 2) { + return 6.0f * ay * t + 2.0f * by; + } + return 0.0f; +} + +static void target_point_approximation(void) { + for (uint8_t i = 0; i < pursuit_state.total_newton_iterations; i++) { + + float last_t_lookahead = pursuit_state.t_lookahead; + + float dx = evaluate_x(last_t_lookahead, 0) - odom_state.global_x; + float dy = evaluate_y(last_t_lookahead, 0) - odom_state.global_y; + float num = (dx * dx) + (dy * dy) - (pursuit_state.lookahead * pursuit_state.lookahead); + float den = 2.0f * dx * evaluate_x(last_t_lookahead, 1) + 2.0f * dy * evaluate_y(last_t_lookahead, 1); + + if (den != 0.0f) { + pursuit_state.t_lookahead = last_t_lookahead - (num / den) / 3.0f; + } + if (pursuit_state.t_lookahead > (float)pursuit_state.total_splines) { + pursuit_state.t_lookahead = 0.0f; + } + } + pursuit_state.target_x = evaluate_x(pursuit_state.t_lookahead, 0); + pursuit_state.target_y = evaluate_y(pursuit_state.t_lookahead, 0); +} + +static float calculate_pure_pursuit(void) { + float world_x_diff = pursuit_state.target_x - odom_state.global_x; + float world_y_diff = pursuit_state.target_y - odom_state.global_y; + float relative_y = (world_y_diff * pb_fast_cos(odom_state.global_h)) - (world_x_diff * pb_fast_sin(odom_state.global_h)); + float dist_sq = (world_x_diff * world_x_diff) + (world_y_diff * world_y_diff); + + if (relative_y > 0.001f || relative_y < -0.001f) { + return -(dist_sq / (2.0f * relative_y)); + } + return 0.0f; +} + +static float evaluate_path_curvature(void) { + float dx = evaluate_x(pursuit_state.t_lookahead, 1); + float dy = evaluate_y(pursuit_state.t_lookahead, 1); + float ddx = evaluate_x(pursuit_state.t_lookahead, 2); + float ddy = evaluate_y(pursuit_state.t_lookahead, 2); + + float curvature = 0.0f; + if (dx != 0.0f || dy != 0.0f) { + float num = fabsf(dx * ddy - dy * ddx); + float den_b = (dx * dx) + (dy * dy); + float den = den_b * sqrtf(den_b); + curvature = num / den; + } + + if (curvature > pursuit_state.max_curvature) { + curvature = pursuit_state.max_curvature; + } + if (curvature < pursuit_state.min_curvature) { + curvature = pursuit_state.min_curvature; + } + return curvature; +} + +static void execute_speed_control(float turning_radius, float path_curvature) { + float local_max_speed = pursuit_state.min_speed + + (path_curvature - pursuit_state.max_curvature) * + (pursuit_state.max_speed - pursuit_state.min_speed) / + (pursuit_state.min_curvature - pursuit_state.max_curvature); + + float local_base_speed = local_max_speed * pursuit_state.base_speed_percentage; + float right_target = 0.0f, left_target = 0.0f; + + if (turning_radius != 0.0f) { + float track_half = (1.0f / odom_state.inv_track) / 2.0f; + right_target = local_base_speed * (turning_radius + track_half) / turning_radius; + left_target = local_base_speed * (turning_radius - track_half) / turning_radius; + } + + float time_passed = (float)odom_state.mstowait / 1000.0f; + float right_accel = right_target - pursuit_state.right_motor_speed; + float left_accel = left_target - pursuit_state.left_motor_speed; + float max_step = pursuit_state.max_per_motor_acceleration * time_passed; + + if (fabsf(right_accel) > 0.0f || fabsf(left_accel) > 0.0f) { + if (fabsf(right_accel) >= fabsf(left_accel)) { + float accel_ratio = left_accel / right_accel; + if (fabsf(right_accel) > max_step) { + right_accel = max_step * (right_accel > 0 ? 1.0f : -1.0f); + } + left_accel = right_accel * accel_ratio; + } else { + float accel_ratio = right_accel / left_accel; + if (fabsf(left_accel) > max_step) { + left_accel = max_step * (left_accel > 0 ? 1.0f : -1.0f); + } + right_accel = left_accel * accel_ratio; + } + } + + pursuit_state.right_motor_speed += right_accel; + pursuit_state.left_motor_speed += left_accel; + + float current_robot_speed = (pursuit_state.right_motor_speed + pursuit_state.left_motor_speed) / 2.0f; + if (current_robot_speed > pursuit_state.max_speed) { + current_robot_speed = pursuit_state.max_speed; + } + if (current_robot_speed < pursuit_state.min_speed) { + current_robot_speed = pursuit_state.min_speed; + } + + pursuit_state.lookahead = pursuit_state.min_lookahead + + (current_robot_speed - pursuit_state.min_speed) * + (pursuit_state.max_lookahead - pursuit_state.min_lookahead) / + (pursuit_state.max_speed - pursuit_state.min_speed); + + pbio_servo_run_forever(odom_state.left_servo, (int32_t)pursuit_state.left_motor_speed); + pbio_servo_run_forever(odom_state.right_servo, (int32_t)pursuit_state.right_motor_speed); +} + +void pb_background_pursuit_update(void) { + if (!pursuit_state.running || !odom_state.left_servo || !odom_state.right_servo) { + return; + } + + uint32_t now = mp_hal_ticks_ms(); + if (now - pursuit_state.last_time_ms < odom_state.mstowait) { + return; + } + pursuit_state.last_time_ms = now; + + if (pursuit_state.t_lookahead >= (float)pursuit_state.total_splines) { + pursuit_state.running = false; + pbio_servo_stop(odom_state.left_servo, PBIO_CONTROL_ON_COMPLETION_BRAKE); + pbio_servo_stop(odom_state.right_servo, PBIO_CONTROL_ON_COMPLETION_BRAKE); + return; + } + + target_point_approximation(); + float turning_radius = calculate_pure_pursuit(); + float path_curvature = evaluate_path_curvature(); + execute_speed_control(turning_radius, path_curvature); +} + +mp_obj_t experimental_start_pursuit(size_t n_args, const mp_obj_t *args) { + size_t num_splines; + mp_obj_t *splines_arr; + mp_obj_get_array(args[0], &num_splines, &splines_arr); + pursuit_state.total_splines = (uint16_t)num_splines; + pursuit_state.spline_coefficients = m_new(float[8], num_splines); + + for (size_t i = 0; i < num_splines; i++) { + size_t num_coeffs; + mp_obj_t *coeffs_arr; + mp_obj_get_array(splines_arr[i], &num_coeffs, &coeffs_arr); + for (size_t j = 0; j < 8; j++) { + pursuit_state.spline_coefficients[i][j] = mp_obj_get_float(coeffs_arr[j]); + } + } + + size_t db_len; + mp_obj_t *db_arr; + mp_obj_get_array(args[1], &db_len, &db_arr); + pursuit_state.max_speed = mp_obj_get_float(db_arr[2]); + pursuit_state.min_speed = mp_obj_get_float(db_arr[3]); + pursuit_state.base_speed_percentage = mp_obj_get_float(db_arr[4]); + pursuit_state.max_per_motor_acceleration = mp_obj_get_float(db_arr[5]); + + size_t tun_len; + mp_obj_t *tun_arr; + mp_obj_get_array(args[2], &tun_len, &tun_arr); + pursuit_state.min_curvature = 1.0f / mp_obj_get_float(tun_arr[0]); + pursuit_state.max_curvature = 1.0f / mp_obj_get_float(tun_arr[1]); + pursuit_state.min_lookahead = mp_obj_get_float(tun_arr[2]); + pursuit_state.max_lookahead = mp_obj_get_float(tun_arr[3]); + pursuit_state.total_newton_iterations = (uint8_t)mp_obj_get_int(tun_arr[4]); + + pursuit_state.t_lookahead = 0.5f; + pursuit_state.lookahead = pursuit_state.min_lookahead; + pursuit_state.left_motor_speed = 0.0f; + pursuit_state.right_motor_speed = 0.0f; + pursuit_state.running = true; + return mp_const_none; +} + +mp_obj_t experimental_stop_pursuit(void) { + pursuit_state.running = false; + if (odom_state.left_servo && odom_state.right_servo) { + pbio_servo_stop(odom_state.left_servo, PBIO_CONTROL_ON_COMPLETION_BRAKE); + pbio_servo_stop(odom_state.right_servo, PBIO_CONTROL_ON_COMPLETION_BRAKE); + } + return mp_const_none; +} + +#endif // PYBRICKS_PY_EXPERIMENTAL diff --git a/pybricks/experimental/pursuit.h b/pybricks/experimental/pursuit.h new file mode 100644 index 000000000..7c90701b8 --- /dev/null +++ b/pybricks/experimental/pursuit.h @@ -0,0 +1,40 @@ +#ifndef PYBRICKS_EXPERIMENTAL_PURSUIT_H +#define PYBRICKS_EXPERIMENTAL_PURSUIT_H + +#include "py/obj.h" +#include +#include + +// pure pursuit state struct (Optimized for Cortex-M4F) +typedef struct { + float (*spline_coefficients)[8]; + volatile uint32_t last_time_ms; + + volatile float t_lookahead; + volatile float lookahead; + volatile float target_x; + volatile float target_y; + + volatile float left_motor_speed; + volatile float right_motor_speed; + + float max_speed; + float min_speed; + float base_speed_percentage; + float max_per_motor_acceleration; + float min_lookahead; + float max_lookahead; + float min_curvature; + float max_curvature; + + uint16_t total_splines; + uint8_t total_newton_iterations; + + volatile bool running; +} pb_pursuit_state_t; + +extern pb_pursuit_state_t pursuit_state; + +void pb_background_pursuit_update(void); + +#endif // PYBRICKS_EXPERIMENTAL_PURSUIT_H From acd7a67e4266276f722772facc03735a0d9f9cb0 Mon Sep 17 00:00:00 2001 From: techcatgato Date: Wed, 29 Jul 2026 18:44:06 +0300 Subject: [PATCH 2/2] added pursuit to common.mk --- bricks/_common/common.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/bricks/_common/common.mk b/bricks/_common/common.mk index 61a0a267d..ff78dbd81 100644 --- a/bricks/_common/common.mk +++ b/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)