Fix default structs, add explicit array indexes

src/common/shared_structs.h

@@ -1,16 +1,20 @@
 #ifndef shared_structs_h
 #define shared_structs_h
 
+typedef enum { MOTOR_1 = 0, MOTOR_2 = 1, MOTOR_3 = 2 } motor_id_t;
+typedef enum { ROLLER_1 = 0, ROLLER_2 = 1, ROLLER_3 = 2 } roller_id_t;
+typedef enum { BIN_1 = 0, BIN_2 = 1, BIN_3 = 2 } bin_id_t;
+
 typedef struct {
-    bool enabled = false;
-    float desired_speed = 0.0;
+    bool enabled;
+    float desired_speed;
 } motor_control_t;
 
 typedef struct {
-    float kp = 0.5;
-    float ki = 0.0;
-    float kd = 0.0;
-    float min_duty = 0.1;
+    float kp;
+    float ki;
+    float kd;
+    float min_duty;
 } motor_parameters_t;
 
 typedef enum { MOTOR_CONTROL_PACKET, MOTOR_PARAMETERS_PACKET } packet_type_t;

src/motor-esp32/i2c_comms.cpp

@@ -17,21 +17,21 @@ void on_I2C_request() {
     // Hopefully because most of our data is pulled in with queues, rather than larger locks,
     // assembling the reply won't take too long.
     
-    i2c_reply.roller_1_speed = get_roller_speed(0);
-    i2c_reply.roller_2_speed = get_roller_speed(1);
-    i2c_reply.roller_3_speed = get_roller_speed(2);
-    i2c_reply.roller_1_revs = get_roller_revs(0);
-    i2c_reply.roller_2_revs = get_roller_revs(1);
-    i2c_reply.roller_3_revs = get_roller_revs(2);
-    i2c_reply.motor_1_duty = get_motor_duty(0);
-    i2c_reply.motor_2_duty = get_motor_duty(1);
-    i2c_reply.motor_3_duty = get_motor_duty(2);
-    i2c_reply.motor_1_control = get_motor_control(0);
-    i2c_reply.motor_2_control = get_motor_control(1);
-    i2c_reply.motor_3_control = get_motor_control(2);
-    i2c_reply.motor_1_params = get_motor_parameters(0);
-    i2c_reply.motor_2_params = get_motor_parameters(1);
-    i2c_reply.motor_3_params = get_motor_parameters(2);
+    i2c_reply.roller_1_speed = get_roller_speed(ROLLER_1);
+    i2c_reply.roller_2_speed = get_roller_speed(ROLLER_2);
+    i2c_reply.roller_3_speed = get_roller_speed(ROLLER_3);
+    i2c_reply.roller_1_revs = get_roller_revs(ROLLER_1);
+    i2c_reply.roller_2_revs = get_roller_revs(ROLLER_2);
+    i2c_reply.roller_3_revs = get_roller_revs(ROLLER_3);
+    i2c_reply.motor_1_duty = get_motor_duty(MOTOR_1);
+    i2c_reply.motor_2_duty = get_motor_duty(MOTOR_2);
+    i2c_reply.motor_3_duty = get_motor_duty(MOTOR_3);
+    i2c_reply.motor_1_control = get_motor_control(MOTOR_1);
+    i2c_reply.motor_2_control = get_motor_control(MOTOR_2);
+    i2c_reply.motor_3_control = get_motor_control(MOTOR_3);
+    i2c_reply.motor_1_params = get_motor_parameters(MOTOR_1);
+    i2c_reply.motor_2_params = get_motor_parameters(MOTOR_2);
+    i2c_reply.motor_3_params = get_motor_parameters(MOTOR_3);
 
     Wire.write((byte *)&i2c_reply, sizeof(i2c_reply));
 }
@@ -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(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);
+            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);
 
         } 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);
+            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);
         } else {
             // I2C error
         }

src/motor-esp32/main.cpp

@@ -18,20 +18,20 @@ motor_control_t m_ctrl;
 void loop() {
     delay(500);
     Serial.print("Dsr:");
-    m_ctrl = get_motor_control(0);
+    m_ctrl = get_motor_control(MOTOR_1);
     Serial.print(m_ctrl.desired_speed);
     Serial.print(" Rsp:");
-    Serial.print(get_roller_speed(0));
+    Serial.print(get_roller_speed(ROLLER_1));
     Serial.print(",");
-    Serial.print(get_roller_speed(1));
+    Serial.print(get_roller_speed(ROLLER_2));
     Serial.print(",");
-    Serial.print(get_roller_speed(2));
+    Serial.print(get_roller_speed(ROLLER_3));
     Serial.print(" Mdty:");
-    Serial.print(get_motor_duty(0));
+    Serial.print(get_motor_duty(MOTOR_1));
     Serial.print(",");
-    Serial.print(get_motor_duty(1));
+    Serial.print(get_motor_duty(MOTOR_2));
     Serial.print(",");
-    Serial.println(get_motor_duty(2));
+    Serial.println(get_motor_duty(MOTOR_3));
 
     // Serial.println(digitalRead(MOTOR_SPEED_SENSE));
     // Serial.print(get_motor_duty(0));
@@ -42,8 +42,8 @@ void loop() {
 
     m_ctrl.enabled = true;
     m_ctrl.desired_speed = ((millis() % 10000) / 10000.0) * 0.65;
-    set_motor_control(0,m_ctrl);
+    set_motor_control(MOTOR_1,m_ctrl);
     m_ctrl.desired_speed = 0.3;
-    set_motor_control(1,m_ctrl);
-    set_motor_control(2,m_ctrl);
+    set_motor_control(MOTOR_2,m_ctrl);
+    set_motor_control(MOTOR_3,m_ctrl);
 }

src/motor-esp32/motor_control.cpp

@@ -50,7 +50,7 @@ static void motor_control_task(void *pvParameters) {
 
             if (m_ctrl.enabled) {
 
-                actual_speed = get_roller_speed(m_id);
+                actual_speed = get_roller_speed((roller_id_t)m_id);
                 error = m_ctrl.desired_speed - actual_speed;
                 m_status[m_id].i_error += error * MC_ITERATION_TIME;
                 d_error = (error - m_status[m_id].last_error) / MC_ITERATION_TIME;
@@ -101,27 +101,27 @@ static void motor_control_task(void *pvParameters) {
     }
 }
 
-void set_motor_control(int motor_id, motor_control_t new_control) {
+void set_motor_control(motor_id_t motor_id, motor_control_t new_control) {
     xQueueOverwrite(motor_control_queue[motor_id], &new_control);
 }
 
-void set_motor_params(int motor_id, motor_parameters_t new_params) {
+void set_motor_params(motor_id_t motor_id, motor_parameters_t new_params) {
     xQueueOverwrite(motor_parameters_queue[motor_id], &new_params);
 }
 
-motor_control_t get_motor_control(int motor_id) {
+motor_control_t get_motor_control(motor_id_t motor_id) {
     motor_control_t current_motor_control;
     xQueuePeek(motor_control_queue[motor_id], &current_motor_control, 0);
     return current_motor_control;
 }
 
-motor_parameters_t get_motor_parameters(int motor_id) {
+motor_parameters_t get_motor_parameters(motor_id_t motor_id) {
     motor_parameters_t current_motor_params;
     xQueuePeek(motor_parameters_queue[motor_id], &current_motor_params, 0);
     return current_motor_params;
 }
 
-float get_motor_duty(int motor_id) {
+float get_motor_duty(motor_id_t motor_id) {
     motor_status_t motor_status;
 
     xQueuePeek(motor_status_queue[motor_id], &motor_status, 0);
@@ -139,12 +139,13 @@ void motor_control_setup(void) {
         // Prime queues - these are single item queues, and so functions reading from them just
         // assume something is there.
         // TODO - this is where we'd read in the saved motor params from flash.
-        motor_control_t init_motor_control;
-        motor_parameters_t init_motor_params;
         motor_status_t blank_motor_status;
+        motor_control_t default_motor_control = {.enabled = false, .desired_speed = 0.0};
+        motor_parameters_t default_motor_parameters = {
+            .kp = DEFAULT_KP, .ki = DEFAULT_KI, .kd = DEFAULT_KD, .min_duty = DEFAULT_MIN_DUTY};
 
-        xQueueOverwrite(motor_control_queue[m_id], &init_motor_control);
-        xQueueOverwrite(motor_parameters_queue[m_id], &init_motor_params);
+        xQueueOverwrite(motor_control_queue[m_id], &default_motor_control);
+        xQueueOverwrite(motor_parameters_queue[m_id], &default_motor_parameters);
         xQueueOverwrite(motor_status_queue[m_id], &blank_motor_status);
     }
 

src/motor-esp32/motor_control.h

@@ -11,17 +11,20 @@
 
 #define MAX_DUTY 0.99 // max duty in each phase - have slightly below 1.0 to give more dead time between phases
 
+#define DEFAULT_KP       0.5
+#define DEFAULT_KI       0.0
+#define DEFAULT_KD       0.0
+#define DEFAULT_MIN_DUTY 0.1
 
+void set_motor_control(motor_id_t motor_id, motor_control_t new_control);
 
-void set_motor_control(int motor_id, motor_control_t new_control);
+void set_motor_params(motor_id_t motor_id, motor_parameters_t new_params);
 
-void set_motor_params(int motor_id, motor_parameters_t new_params);
+motor_control_t get_motor_control(motor_id_t motor_id);
 
-motor_control_t get_motor_control(int motor_id);
+motor_parameters_t get_motor_parameters(motor_id_t motor_id);
 
-motor_parameters_t get_motor_parameters(int motor_id);
-
-float get_motor_duty(int motor_id);
+float get_motor_duty(motor_id_t motor_id);
 
 void motor_control_setup(void);
 

src/motor-esp32/roller_speed.cpp

@@ -3,6 +3,7 @@
 #include "roller_speed.h"
 #include "io.h"
 
+#include "freertos/FreeRTOS.h"
 #include "driver/mcpwm.h"
 #include "freertos/queue.h"
 #include "soc/rtc.h"
@@ -69,7 +70,7 @@ static bool speed_pulse_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id
     return high_task_wakeup == pdTRUE;
 }
 
-float get_roller_speed(int roller_id) {
+float get_roller_speed(roller_id_t roller_id) {
     pulse_measurement_t pulse_measurement;
     // Get the pulse period in APB clock ticks.
     xQueuePeek(roller_measurement_queue[roller_id], &pulse_measurement, 0);
@@ -81,7 +82,7 @@ float get_roller_speed(int roller_id) {
     return ((float)rtc_clk_apb_freq_get() / (float)(pulse_measurement.period)) / (float)(ROLLER_PULSES_PER_REV);
 }
 
-float get_roller_revs(int roller_id) {
+float get_roller_revs(roller_id_t roller_id) {
     pulse_measurement_t pulse_measurement;
     xQueuePeek(roller_measurement_queue[roller_id], &pulse_measurement, 0);
     return (float)pulse_measurement.pulses / (float)ROLLER_PULSES_PER_REV;

src/motor-esp32/roller_speed.h

@@ -1,7 +1,7 @@
 #ifndef roller_speed_h
 #define roller_speed_h
 
-#include "freertos/FreeRTOS.h"
+#include "../common/shared_structs.h"
 
 #define REV_OVERFLOW          10000 // Number at which rev counts overflow back to 0
 #define ROLLER_PULSES_PER_REV 40    // Number of sensor pulses per roller rev
@@ -10,10 +10,10 @@
 
 void roller_speed_setup(void);
 
-// Returns the last calculated speed for roller in revs/second. Only pass 0, 1 or 2
-float get_roller_speed(int roller_id);
+// Returns the last calculated speed for roller in revs/second.
+float get_roller_speed(roller_id_t roller_id);
 
-// Returns the total rev count for a roller. Only pass 0, 1 or 2. Overflows back to 0 at REV_OVERFLOW
-float get_roller_revs(int roller_id);
+// Returns the total rev count for a roller.  Overflows back to 0 at REV_OVERFLOW
+float get_roller_revs(roller_id_t roller_id);
 
 #endif