odo test

Author: techcatgato

pybricks/experimental/pb_module_experimental.c

@@ -10,11 +10,6 @@
 #include "py/runtime.h"
 #include <math.h>
 
-// Direct Hardware Access Headers
-#include <pbio/tacho.h>
-#include <pbio/port.h>
-
-// Architecture Detection & Optimization Macros
 #if defined(__ARM_ARCH_7M__) || defined(__ARM_ARCH_7EM__)
     #define IS_CORTEX_M 1
     #define ACCEL_RAM __attribute__((section(".data"), noinline))
@@ -30,9 +25,8 @@ static const float HALF_PI_F     = 1.570796326794896f;
 static const float INV_TWO_PI_F  = 0.159154943091895f;
 
 // -----------------------------------------------------------------------------
-// Core Math Engines
+// Core Math Engine (Lasse Schlör Absolute Error Optimized)
 // -----------------------------------------------------------------------------
-
 ACCEL_RAM static float fast_sin_internal(float theta) {
     float x = theta * INV_TWO_PI_F;
     x = theta - (float)((int)(x + (x > 0 ? 0.5f : -0.5f))) * TWO_PI_F;
@@ -41,68 +35,79 @@ ACCEL_RAM static float fast_sin_internal(float theta) {
     else if (x < -HALF_PI_F) { x = -PI_F - x; }
 
     float x2 = x * x;
-    float res;
-
     #if IS_CORTEX_M
-        res = -0.0001848814f; 
+        float res = -0.0001848814f; 
         res = 0.0083119000f + (x2 * res);
         res = -0.1666555409f + (x2 * res);
-        res = 1.0f + (x2 * res); // Note: 1.0f is faster for absolute error Minimax
+        return x * (0.9999990609f + (x2 * res));
     #else
-        res = 0.9999990609f + x2 * (-0.1666555409f + x2 * (0.0083119000f + x2 * -0.0001848814f));
+        return x * (0.9999990609f + x2 * (-0.1666555409f + x2 * (0.0083119000f + x2 * -0.0001848814f)));
     #endif
-    
-    return x * res;
+}
+
+// Helper to safely unpack hardware values regardless of Pybricks version
+static float get_float_from_obj(mp_obj_t obj) {
+    if (MP_OBJ_IS_SMALL_INT(obj)) {
+        return (float)MP_OBJ_SMALL_INT_VALUE(obj);
+    } else if (mp_obj_is_type(obj, &mp_type_float)) {
+        return mp_obj_get_float(obj);
+    } else {
+        return (float)mp_obj_get_int(obj);
+    }
 }
 
 // -----------------------------------------------------------------------------
-// Hardware Benchmarks
+// The "No-Tax" Odometry Benchmark
 // -----------------------------------------------------------------------------
-
-static mp_obj_t experimental_odometry_benchmark(mp_obj_t num_iters_in, mp_obj_t wheel_circ_in) {
-    int num_iters = mp_obj_get_int(num_iters_in);
-    float wheel_circ = mp_obj_get_float(wheel_circ_in);
-    float deg_to_mm = wheel_circ / 360.0f;
-
-    float robot_x = 0.0f, robot_y = 0.0f;
-    int32_t last_left = 0, last_right = 0;
-    float heading = 0.0f;
+static mp_obj_t experimental_odometry_benchmark(size_t n_args, const mp_obj_t *args) {
+    // 1. Unpack Python Arguments
+    int num_iters = mp_obj_get_int(args[0]);
+    float wheel_circ = get_float_from_obj(args[1]);
+    mp_obj_t right_angle_func = args[2];
+    mp_obj_t left_angle_func = args[3];
+    mp_obj_t db_angle_func = args[4];
+
+    float deg_to_mm = wheel_circ / 720.0f; 
 
-    // FIX 1: Updated to pbio_tacho_get_angle
-    pbio_tacho_get_angle(pbio_tacho_get_tacho(PBIO_PORT_ID_A), &last_left);
-    pbio_tacho_get_angle(pbio_tacho_get_tacho(PBIO_PORT_ID_D), &last_right);
+    // State variables
+    float robot_x = 0.0f, robot_y = 0.0f;
+    float last_linear = 0.0f;
+    float last_heading = 0.0f;
 
     uint32_t start_time = mp_hal_ticks_ms();
 
+    // 2. The Core High-Speed C Loop
     for (int i = 0; i < num_iters; i++) {
-        int32_t cur_left, cur_right;
-        pbio_tacho_get_angle(pbio_tacho_get_tacho(PBIO_PORT_ID_A), &cur_left);
-        pbio_tacho_get_angle(pbio_tacho_get_tacho(PBIO_PORT_ID_D), &cur_right);
-
-        float d_left = (float)(cur_left - last_left) * deg_to_mm;
-        float d_right = (float)(cur_right - last_right) * deg_to_mm;
-        float linear_delta = (d_left + d_right) / 2.0f;
-        float heading_delta = (d_right - d_left) / 96.0f; // Axle track 96mm
-
-        if (fabsf(linear_delta) > 0.0001f) {
-            float avg_h = heading + (heading_delta / 2.0f);
-            robot_x += linear_delta * fast_sin_internal(avg_h + HALF_PI_F);
-            robot_y += linear_delta * fast_sin_internal(avg_h);
+        
+        // Execute the exact same hardware getters as your Python script
+        float right_angle = get_float_from_obj(mp_call_function_0(right_angle_func));
+        float left_angle  = get_float_from_obj(mp_call_function_0(left_angle_func));
+        float robot_heading = get_float_from_obj(mp_call_function_0(db_angle_func));
+
+        // Exactly mirroring your Python math logic
+        float current_linear = (right_angle + left_angle) * deg_to_mm;
+        float linear = current_linear - last_linear;
+        float heading_difference = robot_heading - last_heading;
+
+        last_linear = current_linear;
+        last_heading = robot_heading;
+
+        if (fabsf(linear) > 0.0f) {
+            float avg_heading_deg = last_heading - (heading_difference / 2.0f);
+            float avg_heading_rad = avg_heading_deg * 0.0174532925f; // DEG_TO_RAD
+            
+            robot_x += linear * fast_sin_internal(avg_heading_rad + HALF_PI_F); // cos
+            robot_y += linear * fast_sin_internal(avg_heading_rad); // sin
         }
 
-        heading += heading_delta;
-        last_left = cur_left;
-        last_right = cur_right;
-
-        // FIX 2 & 3: Semicolon and proper event polling for Pybricks bricks
-        if ((i % 1000) == 0) { 
-            mp_handle_pending(true); 
-        }
+        // Safety: Prevent watchdog timeouts during massive loops
+        if ((i % 1000) == 0) { mp_handle_pending(true); }
     }
 
     uint32_t duration_ms = mp_hal_ticks_ms() - start_time;
     float duration = (float)duration_ms / 1000.0f;
 
+    // 3. Return Results Tuple
     mp_obj_t tuple[5] = {
         mp_obj_new_float_from_f(duration),
         mp_obj_new_int(num_iters),
@@ -112,7 +117,8 @@ static mp_obj_t experimental_odometry_benchmark(mp_obj_t num_iters_in, mp_obj_t
     };
     return mp_obj_new_tuple(5, tuple);
 }
-static MP_DEFINE_CONST_FUN_OBJ_2(experimental_odometry_benchmark_obj, experimental_odometry_benchmark);
+// Define function accepting 5 arguments
+static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_odometry_benchmark_obj, 5, 5, experimental_odometry_benchmark);
 
 // Module Globals
 static const mp_rom_map_elem_t experimental_globals_table[] = {