From 8edde21e8a2218439d21945228910f27db82921a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?V=C3=A1clav=20Jel=C3=ADnek?= <jelinva4@fel.cvut.cz>
Date: Tue, 22 Oct 2024 11:45:46 +0200
Subject: [PATCH] Add reset funciton to motor api
---
micropython/modules/opencube_motors/encoder.c | 5 +++++
micropython/modules/opencube_motors/encoder.h | 6 ++++++
micropython/modules/opencube_motors/motor.c | 5 +++++
micropython/modules/opencube_motors/motor.h | 3 +++
micropython/modules/opencube_motors/motor_api.c | 9 +++++++++
5 files changed, 28 insertions(+)
diff --git a/micropython/modules/opencube_motors/encoder.c b/micropython/modules/opencube_motors/encoder.c
index c18c2be..688e369 100644
--- a/micropython/modules/opencube_motors/encoder.c
+++ b/micropython/modules/opencube_motors/encoder.c
@@ -120,6 +120,11 @@ uint32_t opencube_encoders_get_position(opencube_motor_port port) {
return encoders.ports[port].position;
}
+void opencube_encoders_set_position(opencube_motor_port port, uint32_t position) {
+ // assume that word access is atomic
+ encoders.ports[port].position = position;
+}
+
int32_t opencube_encoders_get_speed(opencube_motor_port port) {
// assume that word access is atomic
return encoders.ports[port].speed;
diff --git a/micropython/modules/opencube_motors/encoder.h b/micropython/modules/opencube_motors/encoder.h
index f6b002b..22f3597 100644
--- a/micropython/modules/opencube_motors/encoder.h
+++ b/micropython/modules/opencube_motors/encoder.h
@@ -26,6 +26,12 @@ extern void opencube_encoders_deinit(int port);
*/
extern uint32_t opencube_encoders_get_position(opencube_motor_port port);
+/**
+ * Set the current position of a motor in the given motor port.
+ * @param position Position in encoder counts (NXT & EV3: these are directly equivalent to degrees).
+ */
+extern void opencube_encoders_set_position(opencube_motor_port port, uint32_t position);
+
/**
* Get the current speed of a motor in the given motor port.
* @return Speed in encoder counts per second (NXT & EV3: these are directly equivalent to degrees per second).
diff --git a/micropython/modules/opencube_motors/motor.c b/micropython/modules/opencube_motors/motor.c
index b76eecc..bd9d7b5 100644
--- a/micropython/modules/opencube_motors/motor.c
+++ b/micropython/modules/opencube_motors/motor.c
@@ -264,6 +264,11 @@ uint32_t opencube_motor_get_position(uint8_t port)
return opencube_encoders_get_position(port);
}
+void opencube_motor_reset_encoder(uint8_t port)
+{
+ opencube_encoders_set_position(port, 0);
+}
+
int32_t opencube_motor_get_speed(uint8_t port)
{
return opencube_encoders_get_speed(port);
diff --git a/micropython/modules/opencube_motors/motor.h b/micropython/modules/opencube_motors/motor.h
index 5f52540..998541f 100644
--- a/micropython/modules/opencube_motors/motor.h
+++ b/micropython/modules/opencube_motors/motor.h
@@ -41,6 +41,9 @@ int32_t opencube_motor_get_speed(uint8_t port);
// Initialize motor encoders
void opencube_motor_encoder_init(void);
+// Reset motor encoder position to 0
+void opencube_motor_reset_encoder(uint8_t port);
+
// Deinitialize motor encoders
void opencube_motor_encoder_deinit(void);
diff --git a/micropython/modules/opencube_motors/motor_api.c b/micropython/modules/opencube_motors/motor_api.c
index 4d617d2..2be1e41 100644
--- a/micropython/modules/opencube_motors/motor_api.c
+++ b/micropython/modules/opencube_motors/motor_api.c
@@ -49,6 +49,14 @@ static mp_obj_t motor_init_encoder(mp_obj_t self_in) {
}
static MP_DEFINE_CONST_FUN_OBJ_1(motor_init_encoder_obj, motor_init_encoder);
+// Reset motor encoder position to 0
+static mp_obj_t motor_reset_encoder(mp_obj_t self_in) {
+ motor_obj_t *self = MP_OBJ_FROM_PTR(self_in);
+ opencube_motor_reset_encoder(self->port);
+ return mp_const_none;
+}
+static MP_DEFINE_CONST_FUN_OBJ_1(motor_reset_encoder_obj, motor_reset_encoder);
+
// Deinitialize motor encoder on all ports
static mp_obj_t motor_deinit_encoder(mp_obj_t self_in) {
opencube_motor_encoder_deinit();
@@ -169,6 +177,7 @@ static const mp_rom_map_elem_t motor_locals_dict[] = {
{ MP_ROM_QSTR(MP_QSTR_deinit), MP_ROM_PTR(&motor_deinit_obj) },
{ MP_ROM_QSTR(MP_QSTR_init_encoder), MP_ROM_PTR(&motor_init_encoder_obj) },
{ MP_ROM_QSTR(MP_QSTR_deinit_encoder), MP_ROM_PTR(&motor_deinit_encoder_obj) },
+ { MP_ROM_QSTR(MP_QSTR_reset_encoder), MP_ROM_PTR(&motor_reset_encoder_obj) },
{ MP_ROM_QSTR(MP_QSTR_position), MP_ROM_PTR(&motor_position_obj) },
{ MP_ROM_QSTR(MP_QSTR_speed), MP_ROM_PTR(&motor_speed_obj) },
{ MP_ROM_QSTR(MP_QSTR_set_regulator_position), MP_ROM_PTR(&motor_set_regulator_position_obj) },
--
GitLab