Add motor control in primary loop

src/primary-esp32/main.cpp

@@ -1,5 +1,11 @@
 #include <Arduino.h>
 
+/*
+Main module for the airseeder controller. Most operation logic is here, written in Arduino style - everything is assumed
+to be in a single thread/task, so don't worry about locks or the multitasking stuff. Where multi-tasking is required,
+that's been bundled out into its own FreeRTOS module (using the ESP IDF libraries), and initialied in setup();
+*/
+
 #include "EspMQTTClient.h"
 
 #include "i2c_comms.h"
@@ -10,6 +16,9 @@
 #define WHEEL_STATUS_UPDATE_PERIOD_MS 1000
 #define HEARTBEAT_PERIOD_MS           1000
 
+// Rate at which the current wheel speed will be used to update the desired roller speed
+#define MOTOR_CONTROL_PERIOD_MS 100
+
 #define POWER_BUTTON_HOLD_MS 300
 
 #define POWER_NO_COMMS_TIMEOUT_MS 1000 * 60 * 5 // 5 minutes
@@ -69,6 +78,8 @@ int parse_message(const String &message) {
     return seg_count;
 }
 
+bool motor_control_required = true;
+
 typedef enum { MODE_DISABLED = 0, MODE_AUTO = 1, MODE_MANUAL = 2 } roller_control_mode_t;
 
 typedef struct {
@@ -108,7 +119,12 @@ void onRollerControlMessage(const String &message) {
         roller_ctrl[1].wheel_ratio = msg_segments[4].toFloat();
         roller_ctrl[2].wheel_ratio = msg_segments[5].toFloat();
 
-        // TODO trip motor control update flag
+        motor_control_required = true;
+
+        sprintf(msg_buffer, "MTR CTRL %u %u %u %.2f %.2f %.2f", roller_ctrl[0].mode, roller_ctrl[1].mode,
+                roller_ctrl[2].mode, roller_ctrl[0].wheel_ratio, roller_ctrl[1].wheel_ratio,
+                roller_ctrl[2].wheel_ratio);
+        mqtt_client.publish("airseeder/status/log", msg_buffer, true);
     }
 }
 
@@ -132,6 +148,10 @@ unsigned long last_wheel_status = 0;
 unsigned long last_heartbeat = 0;
 unsigned long no_comms_timeout_start = 0;
 
+unsigned long last_motor_control_time = 0;
+
+// This is a standard single threaded "frame loop" style of main loop, where time relavent operations check against
+// millis() rather than using delay() calls.
 void loop() {
     now = millis();
 
@@ -203,14 +223,66 @@ void loop() {
         mqtt_client.publish("airseeder/status/heartbeat", msg_buffer, true);
     }
 
-    // Have roller control update required flag. Trip it if something comes in on MQTT or if been longer than update
-    // period. Also trip it if we have anything in manual override mode and now - started > duration If we have new
-    // wheel speed data or if longer than control update, send control update Generate update from current roller modes,
-    // set ratios, and wheel speed Modes and ratios are stored locally here in the main module
+    // Periodically trigger a motor control update
+    if ((now - last_motor_control_time) > MOTOR_CONTROL_PERIOD_MS) {
+        motor_control_required = true;
+    }
 
-    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);
+    // If any rollers are in auto mode and need turning off, trigger a motor control update
+    for (int r_id = 0; r_id < 3; r_id++) {
+        if ((roller_ctrl[r_id].mode == MODE_AUTO) and
+            ((now - roller_ctrl[r_id].manual_time_started) > roller_ctrl[r_id].manual_duration)) {
+            motor_control_required = true;
+        }
+    }
+
+    // If a motor control update needs to be sent to the other motor ESP32, compile the control packet and ask the I2C
+    // task to send it
+    if (motor_control_required) {
+        motor_control_required = false;
+        last_motor_control_time = now;
+
+        motor_control_t m_ctrl[3];
+        float wheel_speed;
+
+        wheel_speed = get_wheel_speed();
+
+        for (int r_id = 0; r_id < 3; r_id++) {
+
+            // If roller is in auto mode, enable the motor if the wheel is moving, and update speed to match desired
+            // ratio
+            if (roller_ctrl[r_id].mode == MODE_AUTO) {
+                if (wheel_speed == 0.0) {
+                    m_ctrl[r_id].enabled = false;
+                    m_ctrl[r_id].desired_speed = 0.0;
+                } else {
+                    m_ctrl[r_id].enabled = true;
+                    m_ctrl[r_id].desired_speed = wheel_speed * roller_ctrl[r_id].wheel_ratio;
+                }
+
+            } else if (roller_ctrl[r_id].mode == MODE_MANUAL) {
+                // If roller is in manual mode, and we are still within the manual period, enable motor and set manual
+                // speed. Otherwise revert to disabled mode.
+                if ((now - roller_ctrl[r_id].manual_time_started) < roller_ctrl[r_id].manual_duration) {
+                    m_ctrl[r_id].enabled = true;
+                    m_ctrl[r_id].desired_speed = roller_ctrl[r_id].manual_speed;
+                } else {
+                    roller_ctrl[r_id].mode = MODE_DISABLED;
+                    m_ctrl[r_id].enabled = false;
+                    m_ctrl[r_id].desired_speed = 0.0;
+                }
+
+            } else {
+                // If we're in the disabled mode, turn off motor
+                m_ctrl[r_id].enabled = false;
+                m_ctrl[r_id].desired_speed = 0.0;
+            }
+        }
+        motor_control_group.motor_1 = m_ctrl[0];
+        motor_control_group.motor_2 = m_ctrl[1];
+        motor_control_group.motor_3 = m_ctrl[2];
+        set_motor_control(motor_control_group);
+    }
 
     mqtt_client.loop();
 }