Skip to content

Commit d110f7d

Browse files
techcatgatotechcatgato
committed
background task
1 parent b334b36 commit d110f7d

4 files changed

Lines changed: 172 additions & 87 deletions

File tree

‎bricks/_common/mphalport.c‎

Lines changed: 22 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,11 @@
1717
#include "py/mpconfig.h"
1818
#include "py/stream.h"
1919

20+
// Bring in our background hook
21+
#if PYBRICKS_PY_EXPERIMENTAL
22+
extern void pb_background_odometry_update(void);
23+
#endif
24+
2025
// Core delay function that does an efficient sleep and may switch thread context.
2126
// We must have the GIL.
2227
void mp_hal_delay_ms(mp_uint_t Delay) {
@@ -28,6 +33,12 @@ void mp_hal_delay_ms(mp_uint_t Delay) {
2833
// raise an exception, switch threads or enter sleep mode (waiting for
2934
// (at least) the SysTick interrupt).
3035
mp_event_wait_indefinite();
36+
37+
// ---> EXPERIMENTAL ODOMETRY HOOK <---
38+
#if PYBRICKS_PY_EXPERIMENTAL
39+
pb_background_odometry_update();
40+
#endif
41+
3142
} while (pbdrv_clock_get_ms() - start < Delay);
3243
}
3344

@@ -49,6 +60,11 @@ int mp_hal_stdin_rx_chr(void) {
4960
// wait for rx interrupt
5061
while (size = 1, pbsys_host_stdin_read(&c, &size) != PBIO_SUCCESS) {
5162
mp_event_wait_indefinite();
63+
64+
// ---> EXPERIMENTAL ODOMETRY HOOK <---
65+
#if PYBRICKS_PY_EXPERIMENTAL
66+
pb_background_odometry_update();
67+
#endif
5268
}
5369

5470
return c;
@@ -76,6 +92,11 @@ mp_uint_t mp_hal_stdout_tx_strn(const char *str, size_t len) {
7692
// Allow long prints to be interrupted.
7793
if (remaining) {
7894
mp_event_wait_indefinite();
95+
96+
// ---> EXPERIMENTAL ODOMETRY HOOK <---
97+
#if PYBRICKS_PY_EXPERIMENTAL
98+
pb_background_odometry_update();
99+
#endif
79100
}
80101
}
81102

@@ -111,4 +132,4 @@ void pb_stdout_flush_to_new_line(void) {
111132

112133
pb_stdout_flush();
113134
}
114-
}
135+
}

‎pybricks/experimental/odometry.c‎

Lines changed: 80 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -12,10 +12,7 @@
1212
#include <pbio/servo.h>
1313
#include "pybricks/experimental/odometry.h"
1414

15-
// THE LIFELINE: Bring in the Pybricks background task runner!
16-
extern bool pbio_os_run_processes_once(void);
17-
18-
// Hardware acceleration for the Spike Prime (FPU + RAM execution)
15+
// Hardware acceleration for the Spike Prime
1916
#if defined(__ARM_ARCH_7M__) || defined(__ARM_ARCH_7EM__)
2017
#define ACCEL_RAM __attribute__((section(".data"), noinline))
2118
#else
@@ -36,76 +33,106 @@ ACCEL_RAM static float fast_sin_internal(float theta) {
3633
return x * (0.99999906f + x2 * (-0.16665554f + x2 * (0.00831190f + x2 * -0.00018488f)));
3734
}
3835

39-
mp_obj_t calculate_odometry(int num_iters, float wheel_circ, float axle_track, mp_obj_t right_motor_obj, mp_obj_t left_motor_obj) {
36+
ACCEL_RAM static float fast_cos_internal(float theta) {
37+
return fast_sin_internal(theta + 1.57079633f);
38+
}
39+
40+
// ---------------------------------------------------------
41+
// BACKGROUND ODOMETRY STATE MACHINE
42+
// ---------------------------------------------------------
43+
volatile bool odom_running = false;
44+
volatile uint32_t last_odom_time_ms = 0;
4045

41-
float deg_to_mm = wheel_circ * 0.0027777778f;
42-
float inv_axle_track = 1.0f / axle_track;
46+
volatile float global_x = 0.0f;
47+
volatile float global_y = 0.0f;
48+
volatile float global_h = 0.0f;
4349

44-
float rx = 0.0f;
45-
float ry = 0.0f;
46-
float rh = 0.0f;
50+
volatile int32_t last_left_angle = 0;
51+
volatile int32_t last_right_angle = 0;
4752

48-
pbio_servo_t *srv_r = ((pb_type_Motor_obj_t *)MP_OBJ_TO_PTR(right_motor_obj))->srv;
49-
pbio_servo_t *srv_l = ((pb_type_Motor_obj_t *)MP_OBJ_TO_PTR(left_motor_obj))->srv;
53+
float odom_deg_to_mm = 1.0f;
54+
float odom_inv_track = 1.0f;
5055

51-
int32_t last_r, last_l, unused_rate;
52-
pbio_servo_get_state_user(srv_r, &last_r, &unused_rate);
53-
pbio_servo_get_state_user(srv_l, &last_l, &unused_rate);
56+
pbio_servo_t *left_servo_ptr = NULL;
57+
pbio_servo_t *right_servo_ptr = NULL;
5458

55-
uint32_t start_time = mp_hal_ticks_ms();
59+
// This is the hook that will run continuously in the OS background
60+
void pb_background_odometry_update(void) {
61+
if (!odom_running) return;
5662

57-
for (int i = 0; i < num_iters; i++) {
58-
int32_t cur_r, cur_l;
59-
60-
// 1. Instant Memory read
61-
pbio_servo_get_state_user(srv_r, &cur_r, &unused_rate);
62-
pbio_servo_get_state_user(srv_l, &cur_l, &unused_rate);
63+
// Throttle to 200Hz (5ms) to prevent starving the motor controllers
64+
uint32_t now = mp_hal_ticks_ms();
65+
if (now - last_odom_time_ms < 5) return;
66+
last_odom_time_ms = now;
6367

64-
// 2. THE BYPASS: Only run heavy FPU math if the wheels physically moved!
65-
int32_t delta_r = cur_r - last_r;
66-
int32_t delta_l = cur_l - last_l;
68+
int32_t cur_l, cur_r, unused_rate;
69+
pbio_servo_get_state_user(left_servo_ptr, &cur_l, &unused_rate);
70+
pbio_servo_get_state_user(right_servo_ptr, &cur_r, &unused_rate);
6771

68-
if (delta_r != 0 || delta_l != 0) {
69-
float dR = (float)delta_r * deg_to_mm;
70-
float dL = (float)delta_l * deg_to_mm;
71-
float dD = (dR + dL) * 0.5f;
72-
float dH = (dR - dL) * inv_axle_track;
72+
int32_t delta_l = cur_l - last_left_angle;
73+
int32_t delta_r = cur_r - last_right_angle;
7374

74-
float avg_h = rh + (dH * 0.5f);
75-
rx += dD * fast_sin_internal(avg_h + 1.5707963f); // cos
76-
ry += dD * fast_sin_internal(avg_h); // sin
77-
rh += dH;
75+
if (delta_l != 0 || delta_r != 0) {
76+
float dL = (float)delta_l * odom_deg_to_mm;
77+
float dR = (float)delta_r * odom_deg_to_mm;
7878

79-
last_r = cur_r;
80-
last_l = cur_l;
81-
}
79+
float dD = (dR + dL) * 0.5f;
80+
float dH = (dR - dL) * odom_inv_track;
81+
float avg_h = global_h + (dH * 0.5f);
8282

83-
// 3. THE LIFELINE: Pump the Pybricks OS so the motors actually drive
84-
// (Bitwise & is a lightning-fast way to do % 256)
85-
if ((i & 0xFF) == 0) {
86-
mp_handle_pending(true);
87-
while (pbio_os_run_processes_once()) {}
88-
}
83+
global_x += dD * fast_cos_internal(avg_h);
84+
global_y += dD * fast_sin_internal(avg_h);
85+
global_h += dH;
86+
87+
last_left_angle = cur_l;
88+
last_right_angle = cur_r;
8989
}
90+
}
91+
92+
// Python API: experimental.start_odometry(left_motor, right_motor, circ, track, x, y, h)
93+
mp_obj_t experimental_start_odometry(size_t n_args, const mp_obj_t *args) {
94+
left_servo_ptr = ((pb_type_Motor_obj_t *)MP_OBJ_TO_PTR(args[0]))->srv;
95+
right_servo_ptr = ((pb_type_Motor_obj_t *)MP_OBJ_TO_PTR(args[1]))->srv;
96+
97+
odom_deg_to_mm = mp_obj_get_float(args[2]) / 360.0f;
98+
odom_inv_track = 1.0f / mp_obj_get_float(args[3]);
99+
100+
global_x = mp_obj_get_float(args[4]);
101+
global_y = mp_obj_get_float(args[5]);
102+
global_h = mp_obj_get_float(args[6]);
103+
104+
int32_t unused_rate;
105+
pbio_servo_get_state_user(left_servo_ptr, (int32_t*)&last_left_angle, &unused_rate);
106+
pbio_servo_get_state_user(right_servo_ptr, (int32_t*)&last_right_angle, &unused_rate);
90107

91-
uint32_t dur = mp_hal_ticks_ms() - start_time;
108+
odom_running = true;
109+
return mp_const_none;
110+
}
92111

93-
mp_obj_t tuple[5] = {
94-
mp_obj_new_float_from_f((float)dur * 0.001f),
95-
mp_obj_new_int(num_iters),
96-
mp_obj_new_float_from_f((float)num_iters / ((float)dur * 0.001f)),
97-
mp_obj_new_float_from_f(rx),
98-
mp_obj_new_float_from_f(ry)
112+
// Python API: experimental.get_odometry() -> (x, y, h)
113+
mp_obj_t experimental_get_odometry(void) {
114+
mp_obj_t tuple[3] = {
115+
mp_obj_new_float_from_f(global_x),
116+
mp_obj_new_float_from_f(global_y),
117+
mp_obj_new_float_from_f(global_h)
99118
};
100-
return mp_obj_new_tuple(5, tuple);
119+
return mp_obj_new_tuple(3, tuple);
120+
}
121+
122+
// Python API: experimental.stop_odometry()
123+
mp_obj_t experimental_stop_odometry(void) {
124+
odom_running = false;
125+
return mp_const_none;
101126
}
102127

103-
// Optimized Pure Pursuit Logic using Dot Products
128+
// ---------------------------------------------------------
129+
// PURE PURSUIT (Unchanged)
130+
// ---------------------------------------------------------
104131
mp_obj_t get_pure_pursuit_multipliers(float tx, float ty, float rx_pos, float ry_pos, float rh_ang, float track) {
105132
float x_dif = tx - rx_pos;
106133
float y_dif = ty - ry_pos;
107134

108-
float cos_h = fast_sin_internal(rh_ang + 1.5707963f);
135+
float cos_h = fast_cos_internal(rh_ang);
109136
float sin_h = fast_sin_internal(rh_ang);
110137

111138
float relative_y = (y_dif * cos_h) - (x_dif * sin_h);

‎pybricks/experimental/pb_module_experimental.c‎

Lines changed: 14 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -4,20 +4,16 @@
44

55
#if PYBRICKS_PY_EXPERIMENTAL
66

7-
// Forward declare the C functions
8-
extern mp_obj_t calculate_odometry(int num_iters, float wheel_circ, float axle_track, mp_obj_t right_motor, mp_obj_t left_motor);
7+
// Forward declare the C functions from odometry.c
8+
extern mp_obj_t experimental_start_odometry(size_t n_args, const mp_obj_t *args);
9+
extern mp_obj_t experimental_get_odometry(void);
10+
extern mp_obj_t experimental_stop_odometry(void);
911
extern mp_obj_t get_pure_pursuit_multipliers(float tx, float ty, float rx_pos, float ry_pos, float rh_ang, float track);
1012

11-
static mp_obj_t experimental_odometry_benchmark(size_t n_args, const mp_obj_t *args) {
12-
return calculate_odometry(
13-
mp_obj_get_int(args[0]),
14-
mp_obj_get_float(args[1]),
15-
mp_obj_get_float(args[2]),
16-
args[3], // Raw Python Motor Object (Right)
17-
args[4] // Raw Python Motor Object (Left)
18-
);
19-
}
20-
static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_odometry_benchmark_obj, 5, 5, experimental_odometry_benchmark);
13+
// Wrap them for MicroPython
14+
static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_start_odometry_obj, 7, 7, experimental_start_odometry);
15+
static MP_DEFINE_CONST_FUN_OBJ_0(experimental_get_odometry_obj, experimental_get_odometry);
16+
static MP_DEFINE_CONST_FUN_OBJ_0(experimental_stop_odometry_obj, experimental_stop_odometry);
2117

2218
static mp_obj_t experimental_pure_pursuit_logic(size_t n_args, const mp_obj_t *args) {
2319
return get_pure_pursuit_multipliers(
@@ -31,10 +27,13 @@ static mp_obj_t experimental_pure_pursuit_logic(size_t n_args, const mp_obj_t *a
3127
}
3228
static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_pure_pursuit_logic_obj, 6, 6, experimental_pure_pursuit_logic);
3329

30+
// Register the module functions
3431
const mp_rom_map_elem_t pb_module_experimental_globals_table[] = {
35-
{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_experimental) },
36-
{ MP_ROM_QSTR(MP_QSTR_odometry_benchmark), MP_ROM_PTR(&experimental_odometry_benchmark_obj) },
37-
{ MP_ROM_QSTR(MP_QSTR_pure_pursuit_logic), MP_ROM_PTR(&experimental_pure_pursuit_logic_obj) },
32+
{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_experimental) },
33+
{ MP_ROM_QSTR(MP_QSTR_start_odometry), MP_ROM_PTR(&experimental_start_odometry_obj) },
34+
{ MP_ROM_QSTR(MP_QSTR_get_odometry), MP_ROM_PTR(&experimental_get_odometry_obj) },
35+
{ MP_ROM_QSTR(MP_QSTR_stop_odometry), MP_ROM_PTR(&experimental_stop_odometry_obj) },
36+
{ MP_ROM_QSTR(MP_QSTR_pure_pursuit_logic),MP_ROM_PTR(&experimental_pure_pursuit_logic_obj) },
3837
};
3938
MP_DEFINE_CONST_DICT(pb_module_experimental_globals, pb_module_experimental_globals_table);
4039

‎pybricks/experimental/platform_math.h‎

Lines changed: 56 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -2,46 +2,84 @@
22
#define PYBRICKS_PLATFORM_MATH_H
33

44
#include <math.h>
5-
6-
// ---------------------------------------------------------
7-
// SPIKE PRIME (Cortex-M4F) - Hardware FPU Minimax
8-
// ---------------------------------------------------------
9-
#if defined(__ARM_ARCH_7M__) || defined(__ARM_ARCH_7EM__)
5+
#include <stdint.h> // Required for uint32_t
106

117
static inline float pb_fast_sin(float theta) {
8+
float x = theta;
9+
10+
// 1. Range Reduction to [-PI, PI]
11+
#if defined(__ARM_ARCH_7M__) || defined(__ARM_ARCH_7EM__)
12+
// Spike Prime (Cortex-M4F) optimized wrap
1213
float x_wrap = theta * 0.159154943f;
13-
float x = theta - (float)((int)(x_wrap + (x_wrap > 0.0f ? 0.5f : -0.5f))) * 6.2831853f;
14+
x = theta - (float)((int)(x_wrap + (x_wrap > 0.0f ? 0.5f : -0.5f))) * 6.2831853f;
15+
#else
16+
//loser ev3 wrap
17+
while (x > 3.14159265f) {
18+
x -= 6.28318531f;
19+
}
20+
while (x < -3.14159265f) {
21+
x += 6.28318531f;
22+
}
23+
#endif
1424

25+
// 2. normalizing
1526
if (x > 1.5707963f) {
1627
x = 3.1415926f - x;
1728
} else if (x < -1.5707963f) {
1829
x = -3.1415926f - x;
1930
}
2031

32+
//minimax approximation
2133
float x2 = x * x;
22-
return x * (0.99999906f + x2 * (-0.16665554f + x2 * (0.00831190f + x2 * -0.00018488f)));
34+
return x * (0.99999906f + x2 * (-0.16665554f + x2 * (0.00831190f + x2 * -0.00018488f))); //horners method to save cycles
35+
2336
}
2437

2538
static inline float pb_fast_cos(float theta) {
2639
return pb_fast_sin(theta + 1.57079633f);
2740
}
2841

2942
// ---------------------------------------------------------
30-
// EV3 (ARM9) - glibc `<math.h>` optimized fallbacks
43+
// skrauzys nightmare ^1.2
3144
// ---------------------------------------------------------
32-
#else
3345

34-
static inline float pb_fast_sin(float theta) {
35-
while (theta > 3.14159265f) theta -= 6.28318531f;
36-
while (theta < -3.14159265f) theta += 6.28318531f;
37-
return sinf(theta);
46+
// use 1 newton iteration without division, using x^1.2 = x^2 * (x^-0.2)^4
47+
static inline float pb_fast_pow_1_2_ultra(float x) {
48+
// protect against zero
49+
if (x > -0.0001f && x < 0.0001f) return 0.0f;
50+
51+
uint32_t i = *(uint32_t*)&x; // evil floating point bit level hacking
52+
i &= 0x7FFFFFFF; // absolute value
53+
54+
uint32_t iy = 1278004968 - (i / 5); // what the fuck?
55+
float y = *(float*)&iy;
56+
57+
float y2 = y * y;
58+
float y4 = y2 * y2;
59+
float y5 = y4 * y;
60+
float y_new = y * (1.2f - 0.2f * x * y5); // 1st iteration
61+
// y_new = y_new * ( ... ); // 2nd iteration, this can be removed
62+
63+
//apply x^1.2 = x^2 * (x^-0.2)^4
64+
float yn2 = y_new * y_new;
65+
float yn4 = yn2 * yn2;
66+
float x2 = x * x;
67+
68+
return x2 * yn4;
3869
}
3970

40-
static inline float pb_fast_cos(float theta) {
41-
while (theta > 3.14159265f) theta -= 6.28318531f;
42-
while (theta < -3.14159265f) theta += 6.28318531f;
43-
return cosf(theta);
71+
//skrauzys nightmare derivative
72+
static inline float pb_fast_s_curve_accel(float x, float a, float b, float c) {
73+
float x2 = x * x;
74+
75+
float u = 3.0f * a * x2 + 2.0f * b * x + c;
76+
77+
float num = 6.0f * a * x + 2.0f * b;
78+
79+
float base = num / ((u * u) + 1.0f);
80+
81+
82+
return 560.0f * pb_fast_pow_1_2_ultra(base); //use the legendary hack
4483
}
4584

46-
#endif
4785
#endif // PYBRICKS_PLATFORM_MATH_H

0 commit comments

Comments
 (0)