units

Author: techcatgato

pybricks/experimental/odometry.c

@@ -1,7 +1,6 @@
 // SPDX-License-Identifier: MIT
 #include "py/mpconfig.h"
 
-// This must match the flag in your mpconfigport.h
 #if PYBRICKS_PY_EXPERIMENTAL
 
 #include "py/mphal.h"
@@ -16,35 +15,27 @@
 #include "pybricks/experimental/odometry.h"
 #include "pybricks/experimental/platform_math.h"
 
-// --- THE FIX: Forward declare the Motor Object structure ---
+// Hardware Object structure declaration
 typedef struct _pb_type_pupdevices_Motor_obj_t {
     mp_obj_base_t base;
     pbio_servo_t *srv; 
 } pb_type_pupdevices_Motor_obj_t;
-// -----------------------------------------------------------
 
-// Hardware Pointers
+// State Variables
 static pbio_servo_t *left_servo_ptr = NULL;
 static pbio_servo_t *right_servo_ptr = NULL;
 
-// Odometry State
 volatile bool odom_running = false;
 volatile uint32_t last_odom_time_ms = 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;
 
-// Pursuit / Spline State
 volatile bool pursuit_running = false;
 volatile float p_target_speed = 0.0f;
 volatile float p_lookahead = 120.0f;
-
-// Spline Coefficients for y = ax^3 + bx^2 + cx + d
-volatile float sp_a = 0.0f, sp_b = 0.0f, sp_c = 0.0f, sp_d = 0.0f;
-volatile float sp_x_end = 0.0f;
-
-// --- Background Updates ---
+volatile float sp_a = 0.0f, sp_b = 0.0f, sp_c = 0.0f, sp_d = 0.0f, sp_x_end = 0.0f;
 
 void pb_background_odometry_update(void) {
     if (!odom_running || !left_servo_ptr || !right_servo_ptr) return;
@@ -71,6 +62,9 @@ void pb_background_odometry_update(void) {
         global_y += dD * pb_fast_sin(avg_h);
         global_h += dH;
 
+        while (global_h > 3.14159f) global_h -= 6.28318f;
+        while (global_h < -3.14159f) global_h += 6.28318f;
+
         last_left_angle = cur_l;
         last_right_angle = cur_r;
     }
@@ -88,38 +82,32 @@ void pb_background_pursuit_update(void) {
 
     float target_x = global_x + p_lookahead;
     if (target_x > sp_x_end) target_x = sp_x_end;
-
-    float tx2 = target_x * target_x;
-    float tx3 = tx2 * target_x;
-    float target_y = (sp_a * tx3) + (sp_b * tx2) + (sp_c * target_x) + sp_d;
+    
+    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 dist_sq = (x_dif * x_dif) + (y_dif * y_dif);
-
     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;
-    float m_right = 1.0f;
-
+    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);
+        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 API ---
-
+// 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;
-
     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]);
@@ -142,11 +130,6 @@ mp_obj_t experimental_get_odometry(void) {
     return mp_obj_new_tuple(3, tuple);
 }
 
-mp_obj_t experimental_stop_odometry(void) {
-    odom_running = false;
-    return mp_const_none;
-}
-
 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]);
@@ -155,7 +138,6 @@ mp_obj_t experimental_start_pursuit(size_t n_args, const mp_obj_t *args) {
     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;
 }
@@ -169,4 +151,9 @@ mp_obj_t experimental_stop_pursuit(void) {
     return mp_const_none;
 }
 
+mp_obj_t experimental_stop_odometry(void) {
+    odom_running = false;
+    return mp_const_none;
+}
+
 #endif // PYBRICKS_PY_EXPERIMENTAL

pybricks/experimental/platform_math.h

@@ -2,7 +2,7 @@
 #define PYBRICKS_PLATFORM_MATH_H
 
 #include <math.h>
-#include <stdint.h> // Required for uint32_t
+#include <stdint.h>
 
 static inline float pb_fast_sin(float theta) {
     float x = theta;
@@ -13,74 +13,63 @@ static inline float pb_fast_sin(float theta) {
     float x_wrap = theta * 0.159154943f;
     x = theta - (float)((int)(x_wrap + (x_wrap > 0.0f ? 0.5f : -0.5f))) * 6.2831853f;
     #else
-    //loser ev3 wrap
-    while (x > 3.14159265f) {
-        x -= 6.28318531f;
-    }
-    while (x < -3.14159265f) {
-        x += 6.28318531f;
-    }
+    // EV3 / ARM9 wrap
+    while (x > 3.14159265f) x -= 6.28318531f;
+    while (x < -3.14159265f) x += 6.28318531f;
     #endif
 
-    // 2. normalizing
-    if (x > 1.5707963f) {
-        x = 3.1415926f - x;
-    } else if (x < -1.5707963f) {
-        x = -3.1415926f - x;
-    }
+    // 2. Normalizing
+    if (x > 1.5707963f) x = 3.1415926f - x;
+    else if (x < -1.5707963f) x = -3.1415926f - x;
 
-    //minimax approximation
+    // 3. Minimax approximation (Horner's method)
     float x2 = x * x;
-    return x * (0.99999906f + x2 * (-0.16665554f + x2 * (0.00831190f + x2 * -0.00018488f))); //horners method to save cycles
-    
+    return x * (0.99999906f + x2 * (-0.16665554f + x2 * (0.00831190f + x2 * -0.00018488f)));
 }
 
 static inline float pb_fast_cos(float theta) {
     return pb_fast_sin(theta + 1.57079633f);
 }
 
-// ---------------------------------------------------------
-// skrauzys nightmare ^1.2
-// ---------------------------------------------------------
-
+// ----------------------------------------------------------------------------
+// Quake III Style - Fast Inverse Square Root (Modified for ^1.2)
+// ----------------------------------------------------------------------------
 static inline float pb_fast_pow_1_2_ultra(float x) {
     if (x > -0.0001f && x < 0.0001f) return 0.0f;
 
-    union { float f; uint32_t i; } conv;
-    conv.f = x;
-
-    uint32_t i = conv.i & 0x7FFFFFFF;         // evil floating point bit level hacking
-    i &= 0x7FFFFFFF;                          // absolute value
-
-    uint32_t iy = 1278004968 - (i / 5);       // what the fuck?
-    conv.i = iy;
-    float y = conv.f;
+    long i;
+    float x2, y;
+    const float threehalfs = 1.5f;
 
-    float y2 = y * y;
-    float y4 = y2 * y2;
-    float y5 = y4 * y;
-    float y_new = y * (1.2f - 0.2f * x * y5); // 1st iteration
-    // y_new = y_new * ( ... );               // 2nd iteration, this can be removed
+    x2 = x * 0.5f;
+    y  = x;
+    i  = * ( long * ) &y;                       // evil floating point bit level hacking
+    i  = 0x5f3759df - ( i >> 1 );               // what the fuck? 
+    y  = * ( float * ) &i;
+    y  = y * ( threehalfs - ( x2 * y * y ) );   // 1st iteration
+    // y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed
 
-    float yn2 = y_new * y_new;
+    // Convert the inverse square root back to the ^1.2 required for the S-Curve
+    float yn2 = y * y;
     float yn4 = yn2 * yn2;
-    float x2 = x * x;
-
-    return x2 * yn4;
+    return (x * x) * yn4;
 }
 
-//skrauzys nightmare derivative
+// Skrauzys nightmare derivative
 static inline float pb_fast_s_curve_accel(float x, float a, float b, float c) {
     float x2 = x * x;
 
+    // Calculate the slope of the cubic spline
     float u = 3.0f * a * x2 + 2.0f * b * x + c;
 
+    // Calculate the second derivative (curvature numerator)
     float num = 6.0f * a * x + 2.0f * b;
 
+    // Base value for the power function
     float base = num / ((u * u) + 1.0f);
-
 
-    return 560.0f * pb_fast_pow_1_2_ultra(base); //use the legendary hack
+    // Apply the legendary hack to find acceleration
+    return 560.0f * pb_fast_pow_1_2_ultra(base); 
 }
 
 #endif // PYBRICKS_PLATFORM_MATH_H