Add reset funciton to motor encoders

menu_programs/i2c_master.py

@@ -23,7 +23,9 @@ def i2c_master_run(robot):
     while True:
         robot.display.fill(0)
         robot.display.centered_text(f"I2C master", 0, 1)
-        if ADDRESS in robot.i2c_master.i2c.scan():
+        scan = robot.i2c_master.i2c.scan()
+        print(scan)
+        if ADDRESS in scan:
             connected = True
             robot.display.text(f"Sending: {chr(data_sending+ASCII_a)}", 0, 16, 1)
             robot.display.text(f"Received: {chr(data_received_parsed)}", 0, 24, 1)
@@ -35,11 +37,11 @@ def i2c_master_run(robot):
             robot.display.text('<', 0, 54, 1)
         robot.display.show()
 
-        if connected:
-            robot.i2c_master.write(ADDRESS, (data_sending + ASCII_a).to_bytes(1, 'big'))
-            data_received = robot.i2c_master.read(0x41, 1)
-            if data_received:
-                data_received_parsed = int.from_bytes(data_received, 'big')
+        print("writing")
+        robot.i2c_master.write(ADDRESS, (data_sending + ASCII_a).to_bytes(1, 'big'))
+        #data_received = robot.i2c_master.read(ADDRESS, 1)
+        #if data_received:
+        #   data_received_parsed = int.from_bytes(data_received, 'big')
 
         buttons = robot.buttons.pressed()
         if buttons[Button.LEFT]:

micropython/modules/opencube_motors/encoder_api.c

@@ -73,6 +73,13 @@ static mp_obj_t encoder_get_speed(mp_obj_t self_in) {
 
 static MP_DEFINE_CONST_FUN_OBJ_1(encoder_get_speed_obj, encoder_get_speed);
 
+static mp_obj_t encoder_reset(mp_obj_t self_in) {
+  encoder_obj_t *self = MP_OBJ_FROM_PTR(self_in);
+  self->ref_position = opencube_encoders_get_position(self->port);
+  return mp_const_none;
+}
+static MP_DEFINE_CONST_FUN_OBJ_1(encoder_reset_obj, encoder_reset);
+
 static mp_obj_t encoder_finalizer(mp_obj_t self_in) {
   encoder_obj_t *self = MP_OBJ_FROM_PTR(self_in);
   opencube_encoders_deinit(self->port);
@@ -84,6 +91,7 @@ static const mp_rom_map_elem_t encoder_locals_dict[] = {
     {MP_ROM_QSTR(MP_QSTR_get_position), MP_ROM_PTR(&encoder_get_position_obj)},
     {MP_ROM_QSTR(MP_QSTR_set_position), MP_ROM_PTR(&encoder_set_position_obj)},
     {MP_ROM_QSTR(MP_QSTR_get_speed),    MP_ROM_PTR(&encoder_get_speed_obj)},
+    {MP_ROM_QSTR(MP_QSTR_reset),        MP_ROM_PTR(&encoder_reset_obj)},
     {MP_ROM_QSTR(MP_QSTR___del__),      MP_ROM_PTR(&encoder_finalizer_obj)},
 };
 static MP_DEFINE_CONST_DICT(encoder_locals_dict_obj, encoder_locals_dict);