restructure please compile

Author: techcatgato

bricks/_common/mphalport.c

@@ -20,6 +20,7 @@
 // Bring in our background hook
 #if PYBRICKS_PY_EXPERIMENTAL
 extern void pb_background_odometry_update(void);
+extern void pb_background_pursuit_update(void); // Add this
 #endif
 
 // Core delay function that does an efficient sleep and may switch thread context.
@@ -37,6 +38,7 @@ void mp_hal_delay_ms(mp_uint_t Delay) {
         // ---> EXPERIMENTAL ODOMETRY HOOK <---
         #if PYBRICKS_PY_EXPERIMENTAL
         pb_background_odometry_update();
+        pb_background_pursuit_update();
         #endif
 
     } while (pbdrv_clock_get_ms() - start < Delay);

pybricks/experimental/micropyhon.mk

@@ -1,9 +1,4 @@
 # Always compile the shared Python wrapper
 PYBRICKS_SRC_C += pybricks/experimental/pb_module_experimental.c
+PYBRICKS_SRC_C += pybricks/experimental/odometry.c
 
-# Routing logic based on PLATFORM
-ifeq ($(PLATFORM), $(filter $(PLATFORM), prime_hub essential_hub technic_hub))
-    PYBRICKS_SRC_C += pybricks/experimental/odometry_cortexm4.c
-else ifeq ($(PLATFORM), ev3dev)
-    PYBRICKS_SRC_C += pybricks/experimental/odometry_arm9.c
-endif

pybricks/experimental/odometry.c

@@ -7,60 +7,33 @@
 #include "py/runtime.h"
 #include <math.h>
 
-// Core Pybricks headers for bare-metal hardware access
 #include <pybricks/common.h>
 #include <pbio/servo.h>
 #include "pybricks/experimental/odometry.h"
+#include "pybricks/experimental/platform_math.h"
 
-// Hardware acceleration for the Spike Prime
-#if defined(__ARM_ARCH_7M__) || defined(__ARM_ARCH_7EM__)
-#define ACCEL_RAM __attribute__((section(".data"), noinline))
-#else
-#define ACCEL_RAM
-#endif
-
-ACCEL_RAM static float fast_sin_internal(float theta) {
-    float x = theta * 0.159154943f;
-    x = theta - (float)((int)(x + (x > 0.00f ? 0.5f : -0.5f))) * 6.2831853f;
-
-    if (x > 1.5707963f) {
-        x = 3.1415926f - x;
-    } else if (x < -1.5707963f) {
-        x = -3.1415926f - x;
-    }
-
-    float x2 = x * x;
-    return x * (0.99999906f + x2 * (-0.16665554f + x2 * (0.00831190f + x2 * -0.00018488f)));
-}
-
-ACCEL_RAM static float fast_cos_internal(float theta) {
-    return fast_sin_internal(theta + 1.57079633f);
-}
+// Hardware Pointers
+static pbio_servo_t *left_servo_ptr = NULL;
+static pbio_servo_t *right_servo_ptr = NULL;
 
-// ---------------------------------------------------------
-// BACKGROUND ODOMETRY STATE MACHINE
-// ---------------------------------------------------------
+// Odometry State
 volatile bool odom_running = false;
 volatile uint32_t last_odom_time_ms = 0;
-
-volatile float global_x = 0.0f;
-volatile float global_y = 0.0f;
-volatile float global_h = 0.0f;
-
-volatile int32_t last_left_angle = 0;
-volatile int32_t last_right_angle = 0;
-
+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;
 float odom_deg_to_mm = 1.0f;
 float odom_inv_track = 1.0f;
 
-pbio_servo_t *left_servo_ptr = NULL;
-pbio_servo_t *right_servo_ptr = NULL;
+// Pursuit State
+volatile bool pursuit_running = false;
+volatile float p_target_x = 0.0f, p_target_y = 0.0f, p_target_speed = 0.0f;
+volatile float p_stop_threshold_sq = 400.0f;
+
+// --- Background Updates ---
 
-// This is the hook that will run continuously in the OS background
 void pb_background_odometry_update(void) {
     if (!odom_running) return;
 
-    // Throttle to 200Hz (5ms) to prevent starving the motor controllers
     uint32_t now = mp_hal_ticks_ms();
     if (now - last_odom_time_ms < 5) return;
     last_odom_time_ms = now;
@@ -75,86 +48,93 @@ void pb_background_odometry_update(void) {
     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 dD = (dR + dL) * 0.5f;
         float dH = (dR - dL) * odom_inv_track;
         float avg_h = global_h + (dH * 0.5f);
 
-        global_x += dD * fast_cos_internal(avg_h);
-        global_y += dD * fast_sin_internal(avg_h);
+        global_x += dD * pb_fast_cos(avg_h);
+        global_y += dD * pb_fast_sin(avg_h);
         global_h += dH;
 
         last_left_angle = cur_l;
         last_right_angle = cur_r;
     }
 }
 
-// Python API: experimental.start_odometry(left_motor, right_motor, circ, track, x, y, h)
+void pb_background_pursuit_update(void) {
+    if (!pursuit_running || !left_servo_ptr || !right_servo_ptr) return;
+
+    float x_dif = p_target_x - global_x;
+    float y_dif = p_target_y - global_y;
+    float dist_sq = (x_dif * x_dif) + (y_dif * y_dif);
+
+    if (dist_sq < p_stop_threshold_sq) {
+        pursuit_running = false;
+        pbio_servo_stop(left_servo_ptr, PBIO_CONTROL_STOP_BRAKE);
+        pbio_servo_stop(right_servo_ptr, PBIO_CONTROL_STOP_BRAKE);
+        return;
+    }
+
+    float relative_y = (y_dif * pb_fast_cos(global_h)) - (x_dif * pb_fast_sin(global_h));
+    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 two_r = 2.0f * radius;
+        float track = 1.0f / odom_inv_track;
+        m_right = two_r / (two_r + track);
+        m_left = two_r / (two_r - track);
+    }
+
+    pbio_servo_run_velocity(left_servo_ptr, (int32_t)(p_target_speed * m_left));
+    pbio_servo_run_velocity(right_servo_ptr, (int32_t)(p_target_speed * m_right));
+}
+
+// --- MicroPython API ---
+
 mp_obj_t experimental_start_odometry(size_t n_args, const mp_obj_t *args) {
     left_servo_ptr = ((pb_type_Motor_obj_t *)MP_OBJ_TO_PTR(args[0]))->srv;
     right_servo_ptr = ((pb_type_Motor_obj_t *)MP_OBJ_TO_PTR(args[1]))->srv;
-    
     odom_deg_to_mm = mp_obj_get_float(args[2]) / 360.0f;
     odom_inv_track = 1.0f / mp_obj_get_float(args[3]);
-    
     global_x = mp_obj_get_float(args[4]);
     global_y = mp_obj_get_float(args[5]);
     global_h = mp_obj_get_float(args[6]);
 
     int32_t unused_rate;
     pbio_servo_get_state_user(left_servo_ptr, (int32_t*)&last_left_angle, &unused_rate);
     pbio_servo_get_state_user(right_servo_ptr, (int32_t*)&last_right_angle, &unused_rate);
-
     odom_running = true;
     return mp_const_none;
 }
 
-// Python API: experimental.get_odometry() -> (x, y, h)
 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_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) };
     return mp_obj_new_tuple(3, tuple);
 }
 
-// Python API: experimental.stop_odometry()
 mp_obj_t experimental_stop_odometry(void) {
     odom_running = false;
     return mp_const_none;
 }
 
-// ---------------------------------------------------------
-// PURE PURSUIT (Unchanged)
-// ---------------------------------------------------------
-mp_obj_t get_pure_pursuit_multipliers(float tx, float ty, float rx_pos, float ry_pos, float rh_ang, float track) {
-    float x_dif = tx - rx_pos;
-    float y_dif = ty - ry_pos;
-
-    float cos_h = fast_cos_internal(rh_ang);
-    float sin_h = fast_sin_internal(rh_ang);
-
-    float relative_y = (y_dif * cos_h) - (x_dif * sin_h);
-
-    float m_left = 1.0f;
-    float m_right = 1.0f;
-    
-    float abs_rel_y = relative_y < 0.0f ? -relative_y : relative_y;
+mp_obj_t experimental_start_pursuit(size_t n_args, const mp_obj_t *args) {
+    p_target_x = mp_obj_get_float(args[0]);
+    p_target_y = mp_obj_get_float(args[1]);
+    p_target_speed = mp_obj_get_float(args[2]);
+    float thresh = mp_obj_get_float(args[3]);
+    p_stop_threshold_sq = thresh * thresh;
+    pursuit_running = true;
+    return mp_const_none;
+}
 
-    if (abs_rel_y > 0.001f) {
-        float dist_sq = (x_dif * x_dif) + (y_dif * y_dif);
-        float radius = -(dist_sq / (2.0f * relative_y));
-        float two_r = 2.0f * radius;
-        m_right = two_r / (two_r + track);
-        m_left = two_r / (two_r - track);
+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_STOP_BRAKE);
+        pbio_servo_stop(right_servo_ptr, PBIO_CONTROL_STOP_BRAKE);
     }
-
-    mp_obj_t tuple[2] = {
-        mp_obj_new_float_from_f(m_left),
-        mp_obj_new_float_from_f(m_right)
-    };
-    return mp_obj_new_tuple(2, tuple);
+    return mp_const_none;
 }
 
 #endif

pybricks/experimental/odometry.h

@@ -3,7 +3,8 @@
 
 #include "py/obj.h"
 
-// The signature must use float to match the core and avoid double-promotion errors
-mp_obj_t calculate_odometry(int num_iters, float wheel_circ, float axle_track, mp_obj_t right_angle_func, mp_obj_t left_angle_func);
+// Background update hooks
+void pb_background_odometry_update(void);
+void pb_background_pursuit_update(void);
 
-#endif // PYBRICKS_EXPERIMENTAL_ODOMETRY_H
+#endif

pybricks/experimental/pb_module_experimental.c

@@ -4,36 +4,27 @@
 
 #if PYBRICKS_PY_EXPERIMENTAL
 
-// Forward declare the C functions from odometry.c
+// Forward declarations
 extern mp_obj_t experimental_start_odometry(size_t n_args, const mp_obj_t *args);
 extern mp_obj_t experimental_get_odometry(void);
 extern mp_obj_t experimental_stop_odometry(void);
-extern mp_obj_t get_pure_pursuit_multipliers(float tx, float ty, float rx_pos, float ry_pos, float rh_ang, float track);
+extern mp_obj_t experimental_start_pursuit(size_t n_args, const mp_obj_t *args);
+extern mp_obj_t experimental_stop_pursuit(void);
 
-// Wrap them for MicroPython
+// Object definitions
 static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_start_odometry_obj, 7, 7, experimental_start_odometry);
 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);
+static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_start_pursuit_obj, 4, 4, experimental_start_pursuit);
+static MP_DEFINE_CONST_FUN_OBJ_0(experimental_stop_pursuit_obj, experimental_stop_pursuit);
 
-static mp_obj_t experimental_pure_pursuit_logic(size_t n_args, const mp_obj_t *args) {
-    return get_pure_pursuit_multipliers(
-        mp_obj_get_float(args[0]), 
-        mp_obj_get_float(args[1]), 
-        mp_obj_get_float(args[2]), 
-        mp_obj_get_float(args[3]), 
-        mp_obj_get_float(args[4]), 
-        mp_obj_get_float(args[5])
-    );
-}
-static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_pure_pursuit_logic_obj, 6, 6, experimental_pure_pursuit_logic);
-
-// Register the module functions
 const mp_rom_map_elem_t pb_module_experimental_globals_table[] = {
     { MP_ROM_QSTR(MP_QSTR___name__),          MP_ROM_QSTR(MP_QSTR_experimental) },
     { MP_ROM_QSTR(MP_QSTR_start_odometry),    MP_ROM_PTR(&experimental_start_odometry_obj) },
     { MP_ROM_QSTR(MP_QSTR_get_odometry),      MP_ROM_PTR(&experimental_get_odometry_obj) },
     { MP_ROM_QSTR(MP_QSTR_stop_odometry),     MP_ROM_PTR(&experimental_stop_odometry_obj) },
-    { MP_ROM_QSTR(MP_QSTR_pure_pursuit_logic),MP_ROM_PTR(&experimental_pure_pursuit_logic_obj) },
+    { MP_ROM_QSTR(MP_QSTR_start_pursuit),     MP_ROM_PTR(&experimental_start_pursuit_obj) },
+    { MP_ROM_QSTR(MP_QSTR_stop_pursuit),      MP_ROM_PTR(&experimental_stop_pursuit_obj) },
 };
 MP_DEFINE_CONST_DICT(pb_module_experimental_globals, pb_module_experimental_globals_table);
 
@@ -42,4 +33,4 @@ const mp_obj_module_t pb_module_experimental = {
     .globals = (mp_obj_dict_t *)&pb_module_experimental_globals,
 };
 
-#endif // PYBRICKS_PY_EXPERIMENTAL
+#endif