Add first speed sensing of roller photointerrupter

src/motor-esp32/main.cpp

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

src/motor-esp32/roller_speed.cpp

@@ -0,0 +1,95 @@
+// Module to record roller and wheel speeds from MCPWM capture interrupts. Not yet sure if a task is required at all.
+
+#include "roller_speed.h"
+#include "io.h"
+
+#include "driver/mcpwm.h"
+#include "freertos/queue.h"
+#include "soc/rtc.h"
+
+static uint32_t roller_pulses[3] = {0, 0, 0};         // Total pulses of roller so far
+static uint32_t roller_last_edge_time[3] = {0, 0, 0}; // Precision timer value of most recent edge
+// Also store timestamp as microseconds since start to avoid overflow of the precision timer when checking for stop
+static int64_t roller_last_timestamp[3] = {0, 0, 0};
+
+// Use a single item queue for each measurement we need to distribute.
+// This avoids needing to create a locked buffer system of our own.
+static xQueueHandle roller_measurement_queue[3];
+
+// struct to hold move our pulse measurements from our ISR via the queue
+typedef struct {
+  uint32_t period;   // period since last pulse in APB clock ticks
+  int64_t timestamp; // time measurement occurred in microseconds since boot
+} pulse_measurement_t;
+
+// Gets called by the MCPWM capture module. Tells us which capture unit it was, which edge, and when it happened
+static bool speed_pulse_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id_t cap_sig,
+                                    const cap_event_data_t *edata, void *arg) {
+
+  // Not strictly necessary to do the wakeup check here, as we shouldn't be blokcing anything. Doesn't hurt though.
+  BaseType_t high_task_wakeup = pdFALSE;
+
+  uint r_id = 0;
+
+  if (cap_sig == MCPWM_SELECT_CAP0) {
+    r_id = 0;
+  } else if (cap_sig == MCPWM_SELECT_CAP1) {
+    r_id = 1;
+  } else if (cap_sig == MCPWM_SELECT_CAP2) {
+    r_id = 2;
+  } else {
+    return false;
+  }
+
+  // Add one to our revs counter, and overflow it if necessary
+  roller_pulses[r_id]++;
+  if ((roller_pulses[r_id] / ROLLER_PULSES_PER_REV) > REV_OVERFLOW) {
+    roller_pulses[0] = 0;
+  }
+
+  pulse_measurement_t new_measurement;
+  int64_t time_now;
+
+  time_now = esp_timer_get_time();
+  // Just make sure we have a previous edge to compare to, and that it was no more than ZERO_SPEED_TIMEOUT_US ago
+  if ((roller_last_timestamp[r_id] != 0) and ((time_now - roller_last_timestamp[r_id]) < ZERO_SPEED_TIMEOUT_US)) {
+    new_measurement.timestamp = time_now;
+    new_measurement.period = edata->cap_value - roller_last_edge_time[r_id];
+    xQueueOverwriteFromISR(roller_measurement_queue[r_id], &new_measurement, &high_task_wakeup);
+  }
+
+  roller_last_edge_time[r_id] = edata->cap_value;
+  roller_last_timestamp[r_id] = time_now;
+
+  return high_task_wakeup == pdTRUE;
+}
+
+float getRoller1Speed(void) {
+  pulse_measurement_t pulse_measurement = {0, 0};
+  // Get the pulse period in APB clock ticks.
+  xQueuePeek(roller_measurement_queue[0], &pulse_measurement, 0);
+  if ((pulse_measurement.period == 0) or
+      ((esp_timer_get_time() - pulse_measurement.timestamp) >= ZERO_SPEED_TIMEOUT_US)) {
+    return 0.0;
+  }
+  // Convert that to revs/sec
+  return ((float)rtc_clk_apb_freq_get() / (float)(pulse_measurement.period)) / (float)(ROLLER_PULSES_PER_REV);
+}
+
+void roller_speed_setup(void) {
+  // Create our measurement queues
+  roller_measurement_queue[0] = xQueueCreate(1, sizeof(pulse_measurement_t));
+  roller_measurement_queue[1] = xQueueCreate(1, sizeof(pulse_measurement_t));
+  roller_measurement_queue[2] = xQueueCreate(1, sizeof(pulse_measurement_t));
+
+  // Set MCPWM0 CAP0 on roller 1 speed, enable pullup input pin
+  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_0, GPIO_ROLLER1_SPEED);
+
+  // configure MCPWM0 CAP0
+  mcpwm_capture_config_t conf = {.cap_edge = MCPWM_POS_EDGE, // trigger on riding edge
+                                 .cap_prescale = 1,          // pulses per interrupt
+                                 .capture_cb = speed_pulse_isr_handler,
+                                 .user_data = NULL};
+  // Enable MCWPM0 CAP0
+  mcpwm_capture_enable_channel(MCPWM_UNIT_0, MCPWM_SELECT_CAP0, &conf);
+}

src/motor-esp32/roller_speed.h

@@ -3,12 +3,10 @@
 
 #include "freertos/FreeRTOS.h"
 
-static const float REV_OVERFLOW = 10000; // Number at which rev counts overflow back to 0
+#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
+#define ZERO_SPEED_TIMEOUT_US 1000000 // Note that with a clock at 80MHz, the pulse period counter overflows every 53 seconds
 
-typedef struct {
-    uint32_t speed;
-    uint32_t timestamp;
-} speed_measurement;
 
 void roller_speed_setup(void);