Add motor control via I2C writes

src/common/shared_structs.h

@@ -13,6 +13,24 @@ typedef struct {
     float min_duty = 0.1;
 } motor_parameters_t;
 
+typedef enum { MOTOR_CONTROL_PACKET, MOTOR_PARAMETERS_PACKET } packet_type_t;
+
+typedef struct {
+    packet_type_t packet_type;
+    union {
+        struct {
+            motor_control_t motor_1_control;
+            motor_control_t motor_2_control;
+            motor_control_t motor_3_control;
+        } control;
+        struct {
+            motor_parameters_t motor_1_params;
+            motor_parameters_t motor_2_params;
+            motor_parameters_t motor_3_params;
+        } params;
+    };
+} motor_write_packet_t;
+
 typedef struct {
     float roller_1_speed;
     float roller_2_speed;

src/motor-esp32/i2c_comms.cpp

@@ -33,10 +33,31 @@ void on_I2C_request() {
     i2c_reply.motor_2_params = get_motor_parameters(1);
     i2c_reply.motor_3_params = get_motor_parameters(2);
 
-    Wire.write((byte *)&i2c_reply, sizeof i2c_reply);
+    Wire.write((byte *)&i2c_reply, sizeof(i2c_reply));
 }
 
+static motor_write_packet_t motor_write_packet;
+
 void on_I2C_receive(int len){
+    if (len == sizeof(motor_write_packet)) {
+        Wire.readBytes((byte *)&motor_write_packet, len);
+        if (motor_write_packet.packet_type == MOTOR_CONTROL_PACKET){
+            // We got a motor control packet
+            set_motor_control(0, motor_write_packet.control.motor_1_control);
+            set_motor_control(1, motor_write_packet.control.motor_1_control);
+            set_motor_control(2, motor_write_packet.control.motor_1_control);
+
+        } else if (motor_write_packet.packet_type == MOTOR_PARAMETERS_PACKET){
+            // We got a motor parameters packet
+            set_motor_params(0, motor_write_packet.params.motor_1_params);
+            set_motor_params(1, motor_write_packet.params.motor_1_params);
+            set_motor_params(2, motor_write_packet.params.motor_1_params);
+        } else {
+            // I2C error
+        }
+    } else {
+        // I2C error
+    }
 
 }