Add motor control to I2C task on primary

src/common/shared_structs.h

@@ -19,19 +19,23 @@ typedef struct {
 
 typedef enum { MOTOR_CONTROL_PACKET, MOTOR_PARAMETERS_PACKET } packet_type_t;
 
+typedef struct {
+    motor_control_t motor_1;
+    motor_control_t motor_2;
+    motor_control_t motor_3;
+} motor_control_group_t;
+
+typedef struct {
+    motor_parameters_t motor_1;
+    motor_parameters_t motor_2;
+    motor_parameters_t motor_3;
+} motor_parameters_group_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_control_group_t control;
+        motor_parameters_group_t params;
     };
 } motor_write_packet_t;
 

src/motor-esp32/i2c_comms.cpp

@@ -43,15 +43,15 @@ void on_I2C_receive(int len){
         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(MOTOR_1, motor_write_packet.control.motor_1_control);
-            set_motor_control(MOTOR_2, motor_write_packet.control.motor_2_control);
-            set_motor_control(MOTOR_3, motor_write_packet.control.motor_3_control);
+            set_motor_control(MOTOR_1, motor_write_packet.control.motor_1);
+            set_motor_control(MOTOR_2, motor_write_packet.control.motor_2);
+            set_motor_control(MOTOR_3, motor_write_packet.control.motor_3);
 
         } else if (motor_write_packet.packet_type == MOTOR_PARAMETERS_PACKET){
             // We got a motor parameters packet
-            set_motor_params(MOTOR_1, motor_write_packet.params.motor_1_params);
-            set_motor_params(MOTOR_2, motor_write_packet.params.motor_2_params);
-            set_motor_params(MOTOR_3, motor_write_packet.params.motor_3_params);
+            set_motor_params(MOTOR_1, motor_write_packet.params.motor_1);
+            set_motor_params(MOTOR_2, motor_write_packet.params.motor_2);
+            set_motor_params(MOTOR_3, motor_write_packet.params.motor_3);
         } else {
             // I2C error
         }

src/motor-esp32/main.cpp

@@ -40,10 +40,10 @@ void loop() {
     // Serial.print(", ");
     // Serial.println(get_roller_speed(0));
 
-    m_ctrl.enabled = true;
+    /*m_ctrl.enabled = true;
     m_ctrl.desired_speed = ((millis() % 10000) / 10000.0) * 0.65;
     set_motor_control(MOTOR_1,m_ctrl);
     m_ctrl.desired_speed = 0.3;
     set_motor_control(MOTOR_2,m_ctrl);
-    set_motor_control(MOTOR_3,m_ctrl);
+    set_motor_control(MOTOR_3,m_ctrl);*/
 }

src/primary-esp32/i2c_comms.cpp

@@ -20,7 +20,8 @@ static TaskHandle_t i2c_comms_task_handle = NULL;
 static SemaphoreHandle_t motor_status_semaphore;
 static xQueueHandle motor_status_queue;
 
-motor_status_packet_t i2c_reply;
+static xQueueHandle motor_control_queue;
+
 
 // Ask the I2C task to get get a status update from the motor ESP32
 void request_motor_status_update() {
@@ -33,8 +34,14 @@ bool get_motor_status_update(motor_status_packet_t *status_packet) {
     return (xQueueReceive(motor_status_queue, status_packet, 0) == pdTRUE);
 }
 
+void set_motor_control(motor_control_group_t new_motor_control){
+    xQueueOverwrite(motor_control_queue, &new_motor_control);
+}
+
 static void i2c_comms_task(void *pvParameters) {
     uint8_t reply_bytecount;
+    motor_status_packet_t i2c_reply;
+    motor_write_packet_t motor_write_packet;
 
     for (;;) {
         // Only need to run this task when there is something to do on the I2C bus
@@ -51,6 +58,14 @@ static void i2c_comms_task(void *pvParameters) {
                 // I2C read error of some sort
             }
         }
+
+        // Send a motor control packet if requested
+        if (xQueueReceive(motor_control_queue, &motor_write_packet.control, 0)==pdTRUE){
+            motor_write_packet.packet_type = MOTOR_CONTROL_PACKET;
+            Wire.beginTransmission(MOTOR_ESP32_SLAVE_I2C_ADDR);
+            Wire.write((byte *)&motor_write_packet, sizeof(motor_write_packet));
+            Wire.endTransmission();
+        }
     }
 }
 
@@ -61,6 +76,7 @@ void i2c_comms_setup(void) {
     // Initialise our queues and semaphores
     motor_status_semaphore = xSemaphoreCreateBinary();
     motor_status_queue = xQueueCreate(1, sizeof(motor_status_packet_t));
+    motor_control_queue = xQueueCreate(1, sizeof(motor_control_group_t));
 
     // Kick off our I2C comms task
     xTaskCreate(i2c_comms_task, "MotorCtrl", I2C_TASK_STACK_SIZE, NULL, 5, &i2c_comms_task_handle);

src/primary-esp32/i2c_comms.h

@@ -8,4 +8,6 @@ void i2c_comms_setup(void);
 bool get_motor_status_update(motor_status_packet_t *status_packet);
 void request_motor_status_update();
 
+void set_motor_control(motor_control_group_t new_motor_control);
+
 #endif

src/primary-esp32/main.cpp

@@ -12,6 +12,8 @@ motor_status_packet_t motor_status;
 unsigned long now = 0;
 unsigned long last_motor_status = 0;
 
+motor_control_group_t motor_control_group;
+
 void loop() {
     now = millis();
 
@@ -23,12 +25,16 @@ void loop() {
 
     if (get_motor_status_update(&motor_status)) {
         Serial.print("Dsr:");
-        Serial.print(motor_status.motor_1_settings.desired_speed);
+        Serial.print(motor_status.motor_1_control.desired_speed);
         Serial.print(" Rsp:");
         Serial.print(motor_status.roller_1_speed);
         Serial.print("   Err:");
-        Serial.print(motor_status.motor_1_settings.desired_speed - motor_status.roller_1_speed);
+        Serial.print(motor_status.motor_1_control.desired_speed - motor_status.roller_1_speed);
         Serial.print(" Mdty:");
         Serial.println(motor_status.motor_1_duty);
     }
+
+    motor_control_group.motor_1.enabled = true;
+    motor_control_group.motor_1.desired_speed = ((millis() % 10000) / 10000.0) * 0.65;
+    set_motor_control(motor_control_group);
 }