compile maybe?

Author: techcatgato

pybricks/experimental/odometry.c

@@ -70,20 +70,22 @@ void pb_background_pursuit_update(void) {
 
     // 1. PROJECT TARGET POINT
     // Use sp_a, sp_b, sp_c, sp_d and p_lookahead to find target_x and target_y
-    float target_x = 0.0f; 
-    float target_y = 0.0f;
+    //TODO
+    //float target_x = 0.0f; 
+    //float target_y = 0.0f;
 
     // 2. CHECK EXIT CONDITION
     // Decide when to set pursuit_running = false (e.g. reaching sp_x_end)
 
     // 3. PURE PURSUIT MATH
     // Calculate m_left and m_right based on target_x/y relative to global_x/y
-    float m_left = 1.0f;
-    float m_right = 1.0f;
+    //float m_left = 1.0f;
+    //float m_right = 1.0f;
 
     // 4. DRIVE
-    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));
+    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 ---
@@ -132,10 +134,11 @@ mp_obj_t experimental_start_spline(size_t n_args, const mp_obj_t *args) {
 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);
+        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;
+    
 }
 
 #endif

pybricks/experimental/platform_math.h

@@ -43,24 +43,25 @@ static inline float pb_fast_cos(float theta) {
 // skrauzys nightmare ^1.2
 // ---------------------------------------------------------
 
-// use 1 newton iteration without division, using x^1.2 = x^2 * (x^-0.2)^4
 static inline float pb_fast_pow_1_2_ultra(float x) {
-    // protect against zero
     if (x > -0.0001f && x < 0.0001f) return 0.0f;
 
-    uint32_t i = *(uint32_t*)&x;              // evil floating point bit level hacking
+    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?
-    float y = *(float*)&iy;
+    conv.i = iy;
+    float y = conv.f;
 
     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
 
-    //apply x^1.2 = x^2 * (x^-0.2)^4
     float yn2 = y_new * y_new;
     float yn4 = yn2 * yn2;
     float x2 = x * x;