Skip to content

Commit d8c6a79

Browse files
techcatgatotechcatgato
committed
odo test
1 parent a18a6fa commit d8c6a79

1 file changed

Lines changed: 79 additions & 22 deletions

File tree

‎pybricks/experimental/pb_module_experimental.c‎

Lines changed: 79 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
// SPDX-License-Identifier: MIT
2-
// Copyright (c) 2018-2021 The Pybricks Authors
2+
// Copyright (c) 2018-2026 The Pybricks Authors
33

44
#include "py/mpconfig.h"
55

@@ -10,44 +10,53 @@
1010
#include "py/runtime.h"
1111
#include <math.h>
1212

13+
// Direct Hardware Access Headers
14+
#include <pbio/tacho.h>
15+
#include <pbio/port.h>
16+
1317
// Architecture Detection & Optimization Macros
1418
#if defined(__ARM_ARCH_7M__) || defined(__ARM_ARCH_7EM__)
1519
#define IS_CORTEX_M 1
1620
#define ACCEL_RAM __attribute__((section(".data"), noinline))
1721
#define DWT_CONTROL (*((volatile uint32_t*)0xE0001000))
1822
#define DWT_CYCCNT (*((volatile uint32_t*)0xE0001004))
19-
#define DEMCR (*((volatile uint32_t*)0xE000EDFC))
23+
#define DEMCR (*((volatile uint32_t*)0xE000EDFC))
2024
#else
2125
#define IS_CORTEX_M 0
2226
#define ACCEL_RAM // EV3/Linux handles RAM loading automatically
2327
#endif
2428

29+
// Constants
2530
static const float PI_F = 3.141592653589793f;
2631
static const float TWO_PI_F = 6.283185307179586f;
2732
static const float HALF_PI_F = 1.570796326794896f;
2833
static const float INV_TWO_PI_F = 0.159154943091895f;
2934

3035
// -----------------------------------------------------------------------------
31-
// Core Math Engines (RAM Accelerated on Spike)
36+
// Core Math Engines (Lasse Schlör Absolute Error MiniMax Coefficients)
3237
// -----------------------------------------------------------------------------
3338

3439
ACCEL_RAM static float fast_sin_internal(float theta) {
40+
// 1. Precise Range Reduction to [-PI, PI]
3541
float x = theta * INV_TWO_PI_F;
3642
x = theta - (float)((int)(x + (x > 0 ? 0.5f : -0.5f))) * TWO_PI_F;
3743

44+
// 2. Quadrant folding to [-PI/2, PI/2]
3845
if (x > HALF_PI_F) { x = PI_F - x; }
3946
else if (x < -HALF_PI_F) { x = -PI_F - x; }
4047

4148
float x2 = x * x;
4249
float res;
4350

4451
#if IS_CORTEX_M
45-
res = -0.000195152f;
46-
res = 0.008332152f + (x2 * res);
47-
res = -0.166666567f + (x2 * res);
48-
res = 1.0f + (x2 * res);
52+
// Spike Prime: Horner's Scheme for VMLA.F32 pipeline
53+
res = -0.0001848814f;
54+
res = 0.0083119000f + (x2 * res);
55+
res = -0.1666555409f + (x2 * res);
56+
res = 0.9999990609f + (x2 * res);
4957
#else
50-
res = 1.0f + x2 * (-0.166666567f + x2 * (0.008332152f + x2 * -0.000195152f));
58+
// EV3: Minimized Absolute Error (~9.33e-7)
59+
res = 0.9999990609f + x2 * (-0.1666555409f + x2 * (0.0083119000f + x2 * -0.0001848814f));
5160
#endif
5261

5362
return x * res;
@@ -72,7 +81,7 @@ ACCEL_RAM static float fast_atan2_internal(float y, float x) {
7281
}
7382

7483
// -----------------------------------------------------------------------------
75-
// Wrappers & Hardware Benchmarks
84+
// Python Wrappers & Hardware Benchmarks
7685
// -----------------------------------------------------------------------------
7786

7887
static mp_obj_t experimental_sin(mp_obj_t theta_in) {
@@ -90,11 +99,63 @@ static mp_obj_t experimental_atan2(mp_obj_t y_in, mp_obj_t x_in) {
9099
}
91100
static MP_DEFINE_CONST_FUN_OBJ_2(experimental_atan2_obj, experimental_atan2);
92101

93-
static mp_obj_t experimental_benchmark_hardware(mp_obj_t seed_in) {
94-
float seed = mp_obj_get_float(seed_in);
102+
// The NEW Hardware-Polling Odometry Benchmark
103+
static mp_obj_t experimental_odometry_benchmark(mp_obj_t num_iters_in, mp_obj_t wheel_circ_in) {
104+
int num_iters = mp_obj_get_int(num_iters_in);
105+
float wheel_circ = mp_obj_get_float(wheel_circ_in);
106+
float deg_to_mm = wheel_circ / 360.0f;
107+
108+
float robot_x = 0.0f, robot_y = 0.0f;
109+
int32_t last_left = 0, last_right = 0;
110+
float heading = 0.0f;
95111

112+
// Initial fetch (Assuming Left=A, Right=D as per your robot)
113+
pbio_tacho_get_count(PBIO_PORT_ID_A, &last_left);
114+
pbio_tacho_get_count(PBIO_PORT_ID_D, &last_right);
115+
116+
uint32_t start_time = mp_hal_ticks_ms();
117+
118+
for (int i = 0; i < num_iters; i++) {
119+
int32_t cur_left, cur_right;
120+
pbio_tacho_get_count(PBIO_PORT_ID_A, &cur_left);
121+
pbio_tacho_get_count(PBIO_PORT_ID_D, &cur_right);
122+
123+
float d_left = (float)(cur_left - last_left) * deg_to_mm;
124+
float d_right = (float)(cur_right - last_right) * deg_to_mm;
125+
float linear_delta = (d_left + d_right) / 2.0f;
126+
float heading_delta = (d_right - d_left) / 96.0f; // 96mm axle track
127+
128+
if (fabsf(linear_delta) > 0.0f) {
129+
float avg_h = heading + (heading_delta / 2.0f);
130+
robot_x += linear_delta * fast_sin_internal(avg_h + HALF_PI_F); // Cos
131+
robot_y += linear_delta * fast_sin_internal(avg_h); // Sin
132+
}
133+
134+
heading += heading_delta;
135+
last_left = cur_left;
136+
last_right = cur_right;
137+
138+
if ((i % 1000) == 0) { MICROPY_EVENT_POLL_HOOK }
139+
}
140+
141+
uint32_t duration_ms = mp_hal_ticks_ms() - start_time;
142+
float duration = duration_ms / 1000.0f;
143+
144+
mp_obj_t tuple[5] = {
145+
mp_obj_new_float_from_f(duration),
146+
mp_obj_new_int(num_iters),
147+
mp_obj_new_float_from_f((float)num_iters / duration),
148+
mp_obj_new_float_from_f(robot_x),
149+
mp_obj_new_float_from_f(robot_y)
150+
};
151+
return mp_obj_new_tuple(5, tuple);
152+
}
153+
static MP_DEFINE_CONST_FUN_OBJ_2(experimental_odometry_benchmark_obj, experimental_odometry_benchmark);
154+
155+
// Original CPU-only benchmark for pure math testing
156+
static mp_obj_t experimental_benchmark_math(mp_obj_t seed_in) {
157+
float seed = mp_obj_get_float(seed_in);
96158
#if IS_CORTEX_M
97-
// Spike Prime High-Res Cycle Counter
98159
DEMCR |= 0x01000000; DWT_CONTROL |= 1;
99160
DWT_CYCCNT = 0;
100161
uint32_t start = DWT_CYCCNT;
@@ -103,26 +164,22 @@ static mp_obj_t experimental_benchmark_hardware(mp_obj_t seed_in) {
103164
__asm volatile ("dsb");
104165
return mp_obj_new_int((DWT_CYCCNT - start) / 2);
105166
#else
106-
// EV3 / Generic: Microsecond-based average
107167
uint32_t t0 = mp_hal_ticks_ms();
108168
volatile float res = seed;
109-
int loops = 50000;
110-
for(int i=0; i<loops; i++) {
111-
res = fast_sin_internal(res + 0.0001f);
112-
}
113-
uint32_t dt_ms = mp_hal_ticks_ms() - t0;
114-
// Return result scaled as "nanoseconds" to match Python expected format
115-
return mp_obj_new_int((dt_ms * 1000000) / loops);
169+
for(int i=0; i<50000; i++) { res = fast_sin_internal(res + 0.0001f); }
170+
return mp_obj_new_int(((mp_hal_ticks_ms() - t0) * 1000000) / 50000);
116171
#endif
117172
}
118-
static MP_DEFINE_CONST_FUN_OBJ_1(experimental_benchmark_hardware_obj, experimental_benchmark_hardware);
173+
static MP_DEFINE_CONST_FUN_OBJ_1(experimental_benchmark_math_obj, experimental_benchmark_math);
119174

175+
// Module Globals Table
120176
static const mp_rom_map_elem_t experimental_globals_table[] = {
121177
{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_experimental) },
122178
{ MP_ROM_QSTR(MP_QSTR_sin), MP_ROM_PTR(&experimental_sin_obj) },
123179
{ MP_ROM_QSTR(MP_QSTR_cos), MP_ROM_PTR(&experimental_cos_obj) },
124180
{ MP_ROM_QSTR(MP_QSTR_atan2), MP_ROM_PTR(&experimental_atan2_obj) },
125-
{ MP_ROM_QSTR(MP_QSTR_benchmark_hardware), MP_ROM_PTR(&experimental_benchmark_hardware_obj) },
181+
{ MP_ROM_QSTR(MP_QSTR_benchmark_math), MP_ROM_PTR(&experimental_benchmark_math_obj) },
182+
{ MP_ROM_QSTR(MP_QSTR_odometry_benchmark), MP_ROM_PTR(&experimental_odometry_benchmark_obj) },
126183
};
127184
static MP_DEFINE_CONST_DICT(pb_module_experimental_globals, experimental_globals_table);
128185

0 commit comments

Comments
 (0)