Skip to content

Commit 902ea6c

Browse files
laurensvalklaurensvalk
committed
pybricks.ev3devices: Add GyroSensor.
Also add calibration routine.
1 parent cc168ca commit 902ea6c

File tree

3 files changed

+70
-29
lines changed

3 files changed

+70
-29
lines changed

‎pybricks/ev3devices.h‎

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -11,16 +11,11 @@
1111
#if PYBRICKS_PY_EV3DEVICES
1212

1313
extern const mp_obj_type_t pb_type_ev3devices_ColorSensor;
14+
extern const mp_obj_type_t pb_type_ev3devices_GyroSensor;
1415
extern const mp_obj_type_t pb_type_ev3devices_InfraredSensor;
1516
extern const mp_obj_type_t pb_type_ev3devices_TouchSensor;
1617
extern const mp_obj_type_t pb_type_ev3devices_UltrasonicSensor;
1718

1819
#endif // PYBRICKS_PY_EV3DEVICES
1920

20-
#if PYBRICKS_PY_EV3DEVDEVICES
21-
22-
extern const mp_obj_type_t pb_type_ev3devices_GyroSensor;
23-
24-
#endif // PYBRICKS_PY_EV3DEVDEVICES
25-
2621
#endif // PYBRICKS_INCLUDED_PYBRICKS_EV3DEVICES_H

‎pybricks/ev3devices/pb_module_ev3devices.c‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -14,12 +14,10 @@ static const mp_rom_map_elem_t ev3devices_globals_table[] = {
1414
{ MP_ROM_QSTR(MP_QSTR_Motor), MP_ROM_PTR(&pb_type_Motor) },
1515
#endif
1616
{ MP_ROM_QSTR(MP_QSTR_ColorSensor), MP_ROM_PTR(&pb_type_ev3devices_ColorSensor) },
17+
{ MP_ROM_QSTR(MP_QSTR_GyroSensor), MP_ROM_PTR(&pb_type_ev3devices_GyroSensor) },
1718
{ MP_ROM_QSTR(MP_QSTR_InfraredSensor), MP_ROM_PTR(&pb_type_ev3devices_InfraredSensor) },
1819
{ MP_ROM_QSTR(MP_QSTR_TouchSensor), MP_ROM_PTR(&pb_type_ev3devices_TouchSensor) },
1920
{ MP_ROM_QSTR(MP_QSTR_UltrasonicSensor), MP_ROM_PTR(&pb_type_ev3devices_UltrasonicSensor)},
20-
#if PYBRICKS_PY_EV3DEVDEVICES
21-
{ MP_ROM_QSTR(MP_QSTR_GyroSensor), MP_ROM_PTR(&pb_type_ev3devices_GyroSensor) },
22-
#endif
2321
};
2422
static MP_DEFINE_CONST_DICT(pb_module_ev3devices_globals, ev3devices_globals_table);
2523

‎pybricks/ev3devices/pb_type_ev3devices_gyrosensor.c‎

Lines changed: 68 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33

44
#include "py/mpconfig.h"
55

6-
#if PYBRICKS_PY_EV3DEVDEVICES
6+
#if PYBRICKS_PY_EV3DEVICES
77

88
#include <pybricks/common.h>
99
#include <pybricks/ev3devices.h>
@@ -18,18 +18,20 @@ typedef struct _ev3devices_GyroSensor_obj_t {
1818
pb_type_device_obj_base_t device_base;
1919
pbio_direction_t direction;
2020
mp_int_t offset;
21+
pbio_os_timer_t timer;
2122
} ev3devices_GyroSensor_obj_t;
2223

23-
// pybricks.ev3devices.GyroSensor (internal) Get new offset for new reset angle
24-
static mp_int_t ev3devices_GyroSensor_get_angle_offset(mp_obj_t self_in, pbio_direction_t direction, mp_int_t new_angle) {
24+
static void ev3devices_GyroSensor_reset_angle_offset(mp_obj_t self_in, pbio_direction_t direction, mp_int_t new_angle) {
2525
// Read raw sensor values
26-
int32_t raw_angle = *(int16_t *)pb_type_device_get_data_blocking(self_in, LEGO_DEVICE_MODE_EV3_GYRO_SENSOR__ANG);
26+
int16_t *data = pb_type_device_get_data(self_in, LEGO_DEVICE_MODE_EV3_GYRO_SENSOR__G_A);
27+
28+
ev3devices_GyroSensor_obj_t *self = MP_OBJ_TO_PTR(self_in);
2729

2830
// Get new offset using arguments and raw values
2931
if (direction == PBIO_DIRECTION_CLOCKWISE) {
30-
return raw_angle - new_angle;
32+
self->offset = data[0] - new_angle;
3133
} else {
32-
return -raw_angle - new_angle;
34+
self->offset = -data[0] - new_angle;
3335
}
3436
}
3537

@@ -41,38 +43,39 @@ static mp_obj_t ev3devices_GyroSensor_make_new(const mp_obj_type_t *type, size_t
4143

4244
ev3devices_GyroSensor_obj_t *self = mp_obj_malloc(ev3devices_GyroSensor_obj_t, type);
4345
pb_type_device_init_class(&self->device_base, port_in, LEGO_DEVICE_TYPE_ID_EV3_GYRO_SENSOR);
46+
47+
// Make sure device is in expected mode.
48+
pb_type_device_get_data_blocking(MP_OBJ_FROM_PTR(self), LEGO_DEVICE_MODE_EV3_GYRO_SENSOR__G_A);
49+
4450
self->direction = pb_type_enum_get_value(direction_in, &pb_enum_type_Direction);
4551

46-
// Read initial angle to as offset.
47-
self->offset = ev3devices_GyroSensor_get_angle_offset(MP_OBJ_FROM_PTR(self), self->direction, 0);
52+
// Read initial angle as offset.
53+
ev3devices_GyroSensor_reset_angle_offset(MP_OBJ_FROM_PTR(self), self->direction, 0);
4854
return MP_OBJ_FROM_PTR(self);
4955
}
5056

5157
// pybricks.ev3devices.GyroSensor.speed
5258
static mp_obj_t ev3devices_GyroSensor_speed(mp_obj_t self_in) {
5359
ev3devices_GyroSensor_obj_t *self = MP_OBJ_TO_PTR(self_in);
54-
int32_t raw_speed = *(int16_t *)pb_type_device_get_data_blocking(self_in, LEGO_DEVICE_MODE_EV3_GYRO_SENSOR__RATE);
55-
56-
// changing modes resets angle to 0
57-
self->offset = 0;
60+
int16_t *data = pb_type_device_get_data(self_in, LEGO_DEVICE_MODE_EV3_GYRO_SENSOR__G_A);
5861

5962
if (self->direction == PBIO_DIRECTION_CLOCKWISE) {
60-
return mp_obj_new_int(raw_speed);
63+
return mp_obj_new_int(data[1]);
6164
} else {
62-
return mp_obj_new_int(-raw_speed);
65+
return mp_obj_new_int(-data[1]);
6366
}
6467
}
6568
static MP_DEFINE_CONST_FUN_OBJ_1(ev3devices_GyroSensor_speed_obj, ev3devices_GyroSensor_speed);
6669

6770
// pybricks.ev3devices.GyroSensor.angle
6871
static mp_obj_t ev3devices_GyroSensor_angle(mp_obj_t self_in) {
6972
ev3devices_GyroSensor_obj_t *self = MP_OBJ_TO_PTR(self_in);
70-
int32_t raw_angle = *(int16_t *)pb_type_device_get_data_blocking(self_in, LEGO_DEVICE_MODE_EV3_GYRO_SENSOR__ANG);
73+
int16_t *data = pb_type_device_get_data(self_in, LEGO_DEVICE_MODE_EV3_GYRO_SENSOR__G_A);
7174

7275
if (self->direction == PBIO_DIRECTION_CLOCKWISE) {
73-
return mp_obj_new_int(raw_angle - self->offset);
76+
return mp_obj_new_int(data[0] - self->offset);
7477
} else {
75-
return mp_obj_new_int(-raw_angle - self->offset);
78+
return mp_obj_new_int(-data[0] - self->offset);
7679
}
7780
}
7881
static MP_DEFINE_CONST_FUN_OBJ_1(ev3devices_GyroSensor_angle_obj, ev3devices_GyroSensor_angle);
@@ -83,16 +86,61 @@ static mp_obj_t ev3devices_GyroSensor_reset_angle(size_t n_args, const mp_obj_t
8386
ev3devices_GyroSensor_obj_t, self,
8487
PB_ARG_REQUIRED(angle));
8588

86-
self->offset = ev3devices_GyroSensor_get_angle_offset(MP_OBJ_FROM_PTR(self), self->direction, pb_obj_get_int(angle_in));
89+
ev3devices_GyroSensor_reset_angle_offset(MP_OBJ_FROM_PTR(self), self->direction, pb_obj_get_int(angle_in));
8790
return mp_const_none;
8891
}
8992
static MP_DEFINE_CONST_FUN_OBJ_KW(ev3devices_GyroSensor_reset_angle_obj, 1, ev3devices_GyroSensor_reset_angle);
9093

94+
static pbio_error_t calibration_thread(pbio_os_state_t *state, mp_obj_t parent_obj) {
95+
96+
ev3devices_GyroSensor_obj_t *self = MP_OBJ_TO_PTR(parent_obj);
97+
pbio_port_lump_dev_t *lump = self->device_base.lump_dev;
98+
99+
pbio_error_t err;
100+
pbio_os_state_t unused;
101+
102+
PBIO_OS_ASYNC_BEGIN(state);
103+
104+
// Kick the LUMP thread to stop.
105+
err = pbio_port_lump_request_reset(lump);
106+
if (err != PBIO_SUCCESS) {
107+
return err;
108+
}
109+
110+
// Port will go through the DCM now. This is a brief, fixed duration where
111+
// we can't get the state.
112+
PBIO_OS_AWAIT_MS(state, &self->timer, 300);
113+
114+
// The sensor initially comes back in mode 0, so this waits for synchronization.
115+
PBIO_OS_AWAIT(state, &unused, pbio_port_lump_is_ready(lump));
116+
117+
// The gyro sensor isn't too happy to get a mode change right away, so
118+
// wait briefly.
119+
PBIO_OS_AWAIT_MS(state, &self->timer, 100);
120+
121+
// Now change to gyro and angle mode and wait until ready.
122+
err = pbio_port_lump_set_mode(lump, LEGO_DEVICE_MODE_EV3_GYRO_SENSOR__G_A);
123+
PBIO_OS_AWAIT(state, &unused, pbio_port_lump_is_ready(lump));
124+
125+
PBIO_OS_ASYNC_END(PBIO_SUCCESS);
126+
}
127+
128+
// pybricks.ev3devices.GyroSensor.calibrate
129+
static mp_obj_t ev3devices_GyroSensor_calibrate(mp_obj_t self_in) {
130+
pb_type_async_t config = {
131+
.parent_obj = self_in,
132+
.iter_once = calibration_thread,
133+
};
134+
return pb_type_async_wait_or_await(&config, NULL, false);
135+
}
136+
static MP_DEFINE_CONST_FUN_OBJ_1(ev3devices_GyroSensor_calibrate_obj, ev3devices_GyroSensor_calibrate);
137+
91138
// dir(pybricks.ev3devices.GyroSensor)
92139
static const mp_rom_map_elem_t ev3devices_GyroSensor_locals_dict_table[] = {
93140
{ MP_ROM_QSTR(MP_QSTR_angle), MP_ROM_PTR(&ev3devices_GyroSensor_angle_obj) },
94-
{ MP_ROM_QSTR(MP_QSTR_speed), MP_ROM_PTR(&ev3devices_GyroSensor_speed_obj) },
141+
{ MP_ROM_QSTR(MP_QSTR_calibrate), MP_ROM_PTR(&ev3devices_GyroSensor_calibrate_obj) },
95142
{ MP_ROM_QSTR(MP_QSTR_reset_angle), MP_ROM_PTR(&ev3devices_GyroSensor_reset_angle_obj) },
143+
{ MP_ROM_QSTR(MP_QSTR_speed), MP_ROM_PTR(&ev3devices_GyroSensor_speed_obj) },
96144
};
97145
static MP_DEFINE_CONST_DICT(ev3devices_GyroSensor_locals_dict, ev3devices_GyroSensor_locals_dict_table);
98146

@@ -103,4 +151,4 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_ev3devices_GyroSensor,
103151
make_new, ev3devices_GyroSensor_make_new,
104152
locals_dict, &ev3devices_GyroSensor_locals_dict);
105153

106-
#endif // PYBRICKS_PY_EV3DEVDEVICES
154+
#endif // PYBRICKS_PY_EV3DEVICES

0 commit comments

Comments
 (0)