Add motor 1 PWM and PID control

src/motor-esp32/main.cpp

@@ -1,21 +1,22 @@
 #include <Arduino.h>
 
 #include "roller_speed.h"
-
-//#define MOTOR_SPEED_SENSE 5
+#include "motor_control.h"
 
 void setup() {
-  //pinMode(MOTOR_SPEED_SENSE, INPUT_PULLUP);
-
   Serial.setTimeout(1);
   Serial.begin(57600);
 
   roller_speed_setup();
+  motor_control_setup();
 }
 
 void loop() {
   delay(100);
+  
   //Serial.println(digitalRead(MOTOR_SPEED_SENSE));
+  Serial.print(get_motor_1_duty());
+  Serial.print(", ");
   Serial.println(getRoller1Speed());
-  //Serial.println("test");
+  set_motor_1(true, 0.3);
 }

src/motor-esp32/motor_control.cpp

@@ -0,0 +1,178 @@
+// Module to run the motor control loop and generate the phase synchronised PWM output for the 3 motors
+
+#include "motor_control.h"
+#include "io.h"
+#include "roller_speed.h"
+
+#include "driver/mcpwm.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+#include "freertos/task.h"
+
+#define MOTOR_PWM_FREQ          1000 // PWM frequency in Hz
+#define MOTOR_CONTROL_PERIOD_MS 100  // Period of the motor control loop
+
+#define MAX_DUTY_RAMP 5.0 // max change in duty per second - where 1.0 is full duty
+
+#define MC_ITERATION_TIME 1.0 / MOTOR_CONTROL_PERIOD_MS
+
+#define MC_TASK_STACK_SIZE 2048
+
+typedef struct {
+  bool enabled = false;
+  float desired_speed = 0.0;
+  float kp = 1.0;
+  float ki = 0.0;
+  float kd = 0.0;
+  float min_duty = 0.1;
+} motor_settings_t;
+
+// Like with the speed sensors, use a single-length queue as a way to safely copy data to another non-synchronised task
+static xQueueHandle motor_settings_queue[3];
+
+typedef struct {
+  float duty = 0.0;
+  float last_error = 0.0;
+  float i_error = 0.0;
+} motor_status_t;
+
+// Work with a local copy, use the queue to send to others
+motor_status_t m_stat[3];
+static xQueueHandle motor_status_queue[3];
+
+const mcpwm_timer_t motor_timer[3] = {MCPWM_TIMER_0, MCPWM_TIMER_1, MCPWM_TIMER_2};
+const gpio_num_t motor_en_pins[3] = {GPIO_ROLLER1_EN, GPIO_ROLLER2_EN, GPIO_ROLLER3_EN};
+
+static void motor_control_task(void *pvParameters) {
+  TickType_t xLastWakeTime;
+  motor_settings_t m_set;
+  float actual_speed;
+  float error;
+  float d_error;
+  float duty_correction;
+  float mapped_duty;
+
+  xLastWakeTime = xTaskGetTickCount();
+  for (;;) {
+
+    for (int m_id = 0; m_id < 1; m_id++) {
+      // Get the latest motor settings
+      xQueuePeek(motor_settings_queue[m_id], &m_set, 0);
+
+      if (m_set.enabled) {
+
+        actual_speed = getRoller1Speed();
+        error = m_set.desired_speed - actual_speed;
+        m_stat[m_id].i_error += error * MC_ITERATION_TIME;
+        d_error = (error - m_stat[m_id].last_error) / MC_ITERATION_TIME;
+        m_stat[m_id].last_error = error;
+
+        duty_correction = m_set.kp * error + m_set.ki * m_stat[m_id].i_error + m_set.kd * d_error;
+
+        // Clip duty correction to the maximum allowed ramp
+        if (duty_correction > (MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
+          duty_correction = (MAX_DUTY_RAMP * MC_ITERATION_TIME);
+        } else if (duty_correction < -(MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
+          duty_correction = -(MAX_DUTY_RAMP * MC_ITERATION_TIME);
+        }
+
+        // Apply duty correction, and clip
+        m_stat[m_id].duty += duty_correction;
+        if (m_stat[m_id].duty < 0.0) {
+          m_stat[m_id].duty = 0.0;
+        } else if (m_stat[m_id].duty > 1.0) {
+          m_stat[m_id].duty = 1.0;
+        }
+
+        // Map duty to fit between min and tri-phase for actual motor PWM
+        mapped_duty = 0.33 * (m_stat[m_id].duty * (1.0 - m_set.min_duty) + m_set.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);
+
+        // Enable driver GPIO en
+        gpio_set_level(motor_en_pins[m_id], 1);
+
+      } else {
+        // Motor disabled, so make sure EN pin is off, and reset cumulative error
+        // _don't_ stop MCPWM timer, as starting it again could glitch the phase
+        m_stat[m_id].i_error = 0;
+        m_stat[m_id].last_error = 0;
+        m_stat[m_id].duty = 0;
+
+        gpio_set_level(motor_en_pins[m_id], 0);
+        mcpwm_set_duty(MCPWM_UNIT_0, motor_timer[m_id], MCPWM_GEN_A, 0.0);
+      }
+
+      // Save the motor status for others to retrieve
+      xQueueOverwrite(motor_status_queue[m_id], &m_stat[m_id]);
+    }
+
+    // Suspend task until the next control loop iteration
+    vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(MOTOR_CONTROL_PERIOD_MS));
+  }
+}
+
+void set_motor_1(bool enabled, float desired_speed) {
+  motor_settings_t new_settings;
+
+  xQueuePeek(motor_settings_queue[0], &new_settings, 0);
+  new_settings.enabled = enabled;
+  new_settings.desired_speed = desired_speed;
+  xQueueOverwrite(motor_settings_queue[0], &new_settings);
+}
+
+float get_motor_1_duty() {
+  motor_status_t motor_status;
+
+  xQueuePeek(motor_status_queue[0], &motor_status, 0);
+
+  return motor_status.duty;
+}
+
+void motor_control_setup(void) {
+
+  // Allocate queues
+  motor_settings_queue[0] = xQueueCreate(1, sizeof(motor_settings_t));
+  motor_settings_queue[1] = xQueueCreate(1, sizeof(motor_settings_t));
+  motor_settings_queue[2] = xQueueCreate(1, sizeof(motor_settings_t));
+
+  motor_status_queue[0] = xQueueCreate(1, sizeof(motor_status_t));
+  motor_status_queue[1] = xQueueCreate(1, sizeof(motor_status_t));
+  motor_status_queue[2] = xQueueCreate(1, sizeof(motor_status_t));
+
+  // Prime queues
+  motor_settings_t init_motor_settings;
+  xQueueOverwrite(motor_settings_queue[0], &init_motor_settings);
+  xQueueOverwrite(motor_settings_queue[1], &init_motor_settings);
+  xQueueOverwrite(motor_settings_queue[2], &init_motor_settings);
+
+  motor_status_t blank_motor_status;
+  xQueueOverwrite(motor_status_queue[0], &blank_motor_status);
+  xQueueOverwrite(motor_status_queue[1], &blank_motor_status);
+  xQueueOverwrite(motor_status_queue[2], &blank_motor_status);
+
+  // setup MCPWM timer
+  mcpwm_config_t pwm_config = {
+      .frequency = MOTOR_PWM_FREQ,
+      .cmpr_a = 0,
+      .cmpr_b = 0,
+      .duty_mode = MCPWM_DUTY_MODE_0,
+      .counter_mode = MCPWM_UP_COUNTER,
+  };
+  mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config);
+
+  // Bind to output
+  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, GPIO_ROLLER1_PWM);
+
+  // Setup EN pins
+  gpio_set_level(GPIO_ROLLER1_EN, 0);
+  gpio_set_level(GPIO_ROLLER2_EN, 0);
+  gpio_set_level(GPIO_ROLLER3_EN, 0);
+
+  gpio_set_direction(GPIO_ROLLER1_EN, GPIO_MODE_OUTPUT);
+  gpio_set_direction(GPIO_ROLLER2_EN, GPIO_MODE_OUTPUT);
+  gpio_set_direction(GPIO_ROLLER3_EN, GPIO_MODE_OUTPUT);
+
+  // Kick off our main motor control task
+  xTaskCreate(motor_control_task, "MotorCtrl", MC_TASK_STACK_SIZE, NULL, 5, NULL);
+}

src/motor-esp32/motor_control.h

@@ -0,0 +1,11 @@
+#ifndef motor_control_h
+#define motor_control_h
+
+
+void set_motor_1(bool enabled, float desired_speed);
+
+void motor_control_setup(void);
+
+float get_motor_1_duty(void);
+
+#endif

src/motor-esp32/roller_speed.cpp

@@ -65,7 +65,7 @@ static bool speed_pulse_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id
 }
 
 float getRoller1Speed(void) {
-  pulse_measurement_t pulse_measurement = {0, 0};
+  pulse_measurement_t pulse_measurement;
   // Get the pulse period in APB clock ticks.
   xQueuePeek(roller_measurement_queue[0], &pulse_measurement, 0);
   if ((pulse_measurement.period == 0) or
@@ -82,6 +82,13 @@ void roller_speed_setup(void) {
   roller_measurement_queue[1] = xQueueCreate(1, sizeof(pulse_measurement_t));
   roller_measurement_queue[2] = xQueueCreate(1, sizeof(pulse_measurement_t));
 
+  // Prime queues
+  pulse_measurement_t blank_measurement = {0,0};
+  xQueueOverwrite(roller_measurement_queue[0], &blank_measurement);
+  xQueueOverwrite(roller_measurement_queue[1], &blank_measurement);
+  xQueueOverwrite(roller_measurement_queue[2], &blank_measurement);
+
+
   // Set MCPWM0 CAP0 on roller 1 speed, enable pullup input pin
   mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_0, GPIO_ROLLER1_SPEED);