Change motor control scheme to allow overlap, add op_offset

src/common/shared_structs.h

@@ -14,7 +14,7 @@ typedef struct {
     float kp;
     float ki;
     float kd;
-    float min_duty;
+    float op_offset;
 } motor_parameters_t;
 
 typedef enum { MOTOR_CONTROL_PACKET, MOTOR_PARAMETERS_PACKET } packet_type_t;

src/motor-esp32/motor_control.cpp

@@ -44,7 +44,6 @@ static void motor_control_task(void *pvParameters) {
     float actual_speed;
     float error;
     float d_error;
-    // float duty_correction;
     float mapped_duty;
 
     xLastWakeTime = xTaskGetTickCount();
@@ -66,7 +65,8 @@ static void motor_control_task(void *pvParameters) {
 
                 old_motor_duty = m_status[m_id].duty;
 
-                m_status[m_id].duty = m_param.kp * error + m_param.ki * m_status[m_id].i_error + m_param.kd * d_error;
+                m_status[m_id].duty = (m_param.kp * error) + (m_param.ki * m_status[m_id].i_error) +
+                                      (m_param.kd * d_error) + m_param.op_offset;
 
                 // Clip duty correction to the maximum allowed ramp
                 if ((m_status[m_id].duty - old_motor_duty) > (MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
@@ -92,8 +92,11 @@ static void motor_control_task(void *pvParameters) {
                     m_status[m_id].i_error = new_i_error;
                 }
 
-                // Map duty to fit between min and tri-phase limit for actual motor PWM
-                mapped_duty = 0.33 * (m_status[m_id].duty * (MAX_DUTY - m_param.min_duty) + m_param.min_duty);
+                // Map the control output to actual motor duty between MIN_DUTY and MAX_DUTY.
+                // These are still phase synchronised 33% apart, so MAX_DUTY needs to be less than 33% if we want to
+                // avoid running two at once.
+
+                mapped_duty = (m_status[m_id].duty * (MAX_DUTY - MIN_DUTY)) + MIN_DUTY;
                 // This function takes duty as a float percent...
                 mcpwm_set_duty(MCPWM_UNIT_0, motor_timer[m_id], MCPWM_GEN_A, mapped_duty * 100.0);
 
@@ -173,7 +176,7 @@ void motor_control_setup(void) {
 
         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};
+            .kp = DEFAULT_KP, .ki = DEFAULT_KI, .kd = DEFAULT_KD, .op_offset = DEFAULT_OP_OFFSET};
         motor_status_t blank_motor_status;
 
         xQueueOverwrite(motor_control_queue[m_id], &default_motor_control);
@@ -189,7 +192,7 @@ void motor_control_setup(void) {
         if (err == ESP_OK) {
             xQueueOverwrite(motor_parameters_queue[m_id], &stored_motor_parameters);
         }
-        }
+    }
 
     // Setup MCPWM timers.
     mcpwm_config_t pwm_config = {

src/motor-esp32/motor_control.h

@@ -9,12 +9,13 @@
 
 #define MAX_DUTY_RAMP 10.0 // max change in duty per second - where 1.0 is full duty
 
-#define MAX_DUTY 0.99 // max duty in each phase - have slightly below 1.0 to give more dead time between phases
+#define MIN_DUTY 0.10 // min duty for each motor
+#define MAX_DUTY 0.99 // max duty for each motor - Needs to be below 0.33 to avoid overlapping the three motor phases
 
-#define DEFAULT_KP       0.0
-#define DEFAULT_KI       5.0
-#define DEFAULT_KD       0.0
-#define DEFAULT_MIN_DUTY 0.1
+#define DEFAULT_KP        0.0
+#define DEFAULT_KI        5.0
+#define DEFAULT_KD        0.0
+#define DEFAULT_OP_OFFSET 0.0
 
 void set_motor_control(motor_id_t motor_id, motor_control_t new_control);
 

src/motor-esp32/roller_speed.h

@@ -8,7 +8,7 @@
 
 // Time without a pulse after which the speed will be considered to be 0.
 // Note that with a clock at 80MHz, the pulse period counter overflows every 53 seconds
-#define ZERO_SPEED_TIMEOUT_US 1000000
+#define ZERO_SPEED_TIMEOUT_US 5000000
 
 void roller_speed_setup(void);
 

src/primary-esp32/main.cpp

@@ -211,7 +211,7 @@ void onRollerParamsMessage(const String &topicStr, const String &message) {
             motor_parameters[r_id].kp = msg_segments[0].toFloat();
             motor_parameters[r_id].ki = msg_segments[1].toFloat();
             motor_parameters[r_id].kd = msg_segments[2].toFloat();
-            motor_parameters[r_id].min_duty = msg_segments[3].toFloat();
+            motor_parameters[r_id].op_offset = msg_segments[3].toFloat();
 
             motor_parameters_to_send.motor_1 = motor_parameters[0];
             motor_parameters_to_send.motor_2 = motor_parameters[1];
@@ -450,12 +450,12 @@ void loop() {
         motor_parameter_mqtt_update_required = false;
         last_motor_parameter_update = now;
 
-        //[kp] [ki] [kd] [min_duty]
-        sprintf(msg_buffer, "%.2f %.2f %.2f %.2f", motor_parameters[0].kp, motor_parameters[0].ki, motor_parameters[0].kd, motor_parameters[0].min_duty);
+        //[kp] [ki] [kd] [op_offset]
+        sprintf(msg_buffer, "%.2f %.2f %.2f %.2f", motor_parameters[0].kp, motor_parameters[0].ki, motor_parameters[0].kd, motor_parameters[0].op_offset);
         mqtt_client.publish("airseeder/status/roller1params", msg_buffer, true);
-        sprintf(msg_buffer, "%.2f %.2f %.2f %.2f", motor_parameters[1].kp, motor_parameters[1].ki, motor_parameters[1].kd, motor_parameters[1].min_duty);
+        sprintf(msg_buffer, "%.2f %.2f %.2f %.2f", motor_parameters[1].kp, motor_parameters[1].ki, motor_parameters[1].kd, motor_parameters[1].op_offset);
         mqtt_client.publish("airseeder/status/roller2params", msg_buffer, true);
-        sprintf(msg_buffer, "%.2f %.2f %.2f %.2f", motor_parameters[2].kp, motor_parameters[2].ki, motor_parameters[2].kd, motor_parameters[2].min_duty);
+        sprintf(msg_buffer, "%.2f %.2f %.2f %.2f", motor_parameters[2].kp, motor_parameters[2].ki, motor_parameters[2].kd, motor_parameters[2].op_offset);
         mqtt_client.publish("airseeder/status/roller3params", msg_buffer, true);
     }