Add reset funciton to motor api

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;

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).

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);

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);
 

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) },