diff --git a/main controller code/src/motors.c b/main controller code/src/motors.c
index 8013717..411865b 100644
--- a/main controller code/src/motors.c	
+++ b/main controller code/src/motors.c	
@@ -22,19 +22,19 @@ void i2c_set_motor_speed(motors_enum_t motor, int8_t value)
     const motor_def_t *motor_def = &MOTORS_DEFS[motor];
 
     uint8_t x = *(uint8_t *)&value;
-    uint8_t data[2] = {motor_def->buffer_address, x};
+    uint8_t data[] = {motor_def->buffer_address, x};
 
-    i2c_master_send(I2C_MOTION_CONTROLLER_ADDRESS, data, 2);
+    i2c_master_write(I2C_MOTION_CONTROLLER_ADDRESS, data, 2);
 }
 
 int8_t i2c_get_motor_speed(motors_enum_t motor)
 {
     const motor_def_t *motor_def = &MOTORS_DEFS[motor];
 
-    uint8_t data[1] = {motor_def->buffer_address};
+    uint8_t reg = motor_def->buffer_address; // Register of the i2c_buffer to read
+    uint8_t data;
+    i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, reg, &data, 1);
 
-    i2c_master_send_receive(I2C_MOTION_CONTROLLER_ADDRESS, data, 1);
-
-    int8_t value = *(int8_t *)data;
+    int8_t value = *(int8_t *)&data;
     return value;
 }