How to read motor encoder values with interrupts in FreeRTOS? - c++

Question

I am working on a project where I need to obtain precise angular velocity from four motor encoders. I am using ESP32 DEVKIT-V1 module, and would like to use four interrupts, which will fire when each motor encoder switches state. This produces a square signal of around 700 Hz (period of 1,42 ms). This needs to be done on one core due to timing restrictions, as the processor must not miss any ticks. This is why I decided to use FreeRTOS. As the tick rate of the ESP32 is 1 ms, it cannot read higher frequencies than 500 Hz (period of 2 ms).
I would like to call getEncoderTickNumber() function every time one of the four interrupts fires, however, I only get the ESP32 to continually reset. I also wish to pass the number of ticks (encoderValueA1 - A4) from function getEncoderTickNumber() to getEncoderRPM() by queues.
I am still a beginner in C/C++, so I would be very grateful if you could point out some beginner mistakes that I am making. Thank you for your time.
#include <Arduino.h>
// Motor encoder output pulse per rotation (AndyMark Neverest 60)
int ENC_COUNT_REV = 420;
// Pulse count from encoder
long encoderValueA1 = 0;
long encoderValueA2 = 0;
long encoderValueA3 = 0;
long encoderValueA4 = 0;
int currentStateMotorEncoderA1;
int currentStateMotorEncoderA2;
int currentStateMotorEncoderA3;
int currentStateMotorEncoderA4;
int previousStateMotorEncoderA1;
int previousStateMotorEncoderA2;
int previousStateMotorEncoderA3;
int previousStateMotorEncoderA4;
// Variable for RPM measuerment
int rpm1 = 0;
int rpm2 = 0;
int rpm3 = 0;
int rpm4 = 0;
#define INT_PIN1 17
#define INT_PIN2 18
#define INT_PIN3 19
#define INT_PIN4 16
#define PRIORITY_LOW 0
#define PRIORITY_HIGH 1
QueueHandle_t encoderQueueHandle;
#define QUEUE_LENGTH 4 //four rpm readings
long* pdata = &encoderValueA1;
void io_expander_interrupt()
{
xQueueSendToBackFromISR(&encoderQueueHandle, &pdata, NULL);
}
///////////
// TASKS //
///////////
void getEncoderTickNumber(void *parameter)
{
while (1)
{
if (xQueueReceiveFromISR(&encoderQueueHandle, &pdata, NULL) == pdTRUE)
{
currentStateMotorEncoderA1 = digitalRead(INT_PIN1);
currentStateMotorEncoderA2 = digitalRead(INT_PIN2);
currentStateMotorEncoderA3 = digitalRead(INT_PIN3);
currentStateMotorEncoderA4 = digitalRead(INT_PIN4);
if (currentStateMotorEncoderA1 != previousStateMotorEncoderA1)
{
encoderValueA1++;
}
if (currentStateMotorEncoderA2 != previousStateMotorEncoderA2)
{
encoderValueA2++;
}
if (currentStateMotorEncoderA3 != previousStateMotorEncoderA3)
{
encoderValueA3++;
}
if (currentStateMotorEncoderA4 != previousStateMotorEncoderA4)
{
encoderValueA4++;
}
previousStateMotorEncoderA1 = currentStateMotorEncoderA1;
previousStateMotorEncoderA2 = currentStateMotorEncoderA2;
previousStateMotorEncoderA3 = currentStateMotorEncoderA3;
previousStateMotorEncoderA4 = currentStateMotorEncoderA4;
}
}
}
void getEncoderRPM(void *parameter)
{
while (1)
{
rpm1 = (encoderValueA1 * 60) / ENC_COUNT_REV;
rpm2 = (encoderValueA2 * 60) / ENC_COUNT_REV;
rpm3 = (encoderValueA3 * 60) / ENC_COUNT_REV;
rpm4 = (encoderValueA4 * 60) / ENC_COUNT_REV;
encoderValueA1 = 0;
encoderValueA2 = 0;
encoderValueA3 = 0;
encoderValueA4 = 0;
vTaskDelay(1000 / portTICK_RATE_MS);
}
}
void printData(void *parameter)
{
while (1)
{
Serial.print("1:");
Serial.print(rpm1);
Serial.print(" 2:");
Serial.print(rpm2);
Serial.print(" 3:");
Serial.print(rpm3);
Serial.print(" 4:");
Serial.println(rpm4);
vTaskDelay(500 / portTICK_RATE_MS);
}
}
void setup()
{
Serial.begin(115200);
pinMode(INT_PIN1, INPUT);
attachInterrupt(INT_PIN1, getEncoderTickNumber, RISING);
pinMode(INT_PIN2, INPUT);
attachInterrupt(INT_PIN2, getEncoderTickNumber, RISING);
pinMode(INT_PIN3, INPUT);
attachInterrupt(INT_PIN3, getEncoderTickNumber, RISING);
pinMode(INT_PIN4, INPUT);
attachInterrupt(INT_PIN4, getEncoderTickNumber, RISING);
// Create the queue
encoderQueueHandle = xQueueCreate(QUEUE_LENGTH, sizeof(uint32_t));
xTaskCreatePinnedToCore( // Use xTaskCreate() in vanilla FreeRTOS
getEncoderTickNumber, // Function to be called
"getEncoderTickNumber", // Name of task
1024, // Stack size (bytes in ESP32, words in FreeRTOS) inside the heap
NULL, // Parameter to pass to function
PRIORITY_LOW, // Task priority (0 to configMAX_PRIORITIES - 1)
NULL, // Task handle
1); // Run on one core for demo purposes (ESP32 only)
xTaskCreatePinnedToCore( // Use xTaskCreate() in vanilla FreeRTOS
printData, // Function to be called
"printData", // Name of task
1024, // Stack size (bytes in ESP32, words in FreeRTOS) inside the heap
NULL, // Parameter to pass to function
PRIORITY_LOW, // Task priority (0 to configMAX_PRIORITIES - 1)
NULL, // Task handle
0); // Run on one core for demo purposes (ESP32 only)
xTaskCreatePinnedToCore( // Use xTaskCreate() in vanilla FreeRTOS
getEncoderRPM, // Function to be called
"getEncoderRPM", // Name of task
1024, // Stack size (bytes in ESP32, words in FreeRTOS)
NULL, // Parameter to pass to function
PRIORITY_HIGH, // Task priority (0 to configMAX_PRIORITIES - 1)
NULL, // Task handle
0); // Run on one core for demo purposes (ESP32 only)
vTaskDelete(NULL); // Deletes the setup/loop task now that we are finished setting up (optional)
}
void loop()
{
}

Answer 1

There are quite a few problems in your code. Let's go over them one by one, see if it clears things up.
Firstly, don't delete the task in setup():
vTaskDelete(NULL); // Deletes the setup/loop task now that we are finished setting up (optional)
Arduino will manage the FreeRTOS tasks on its own, don't interfere with it. You may be causing your crash with that line alone.
Secondly, you're creating your tasks with a stack size of 1024 bytes which is too small. The task will likely corrupt the stack and crash. Start with a stack size of 4096 bytes for simple tasks, see if you can optimize later. Incidentally, you don't need any tasks at all for a simple implementation.
Thirdly, you don't seem to understand what an interrupt is and how to handle it. By calling this you're attaching the function getEncoderTickNumber() as an interrupt handler to all 4 GPIO inputs:
attachInterrupt(INT_PIN1, getEncoderTickNumber, RISING);
attachInterrupt(INT_PIN2, getEncoderTickNumber, RISING);
attachInterrupt(INT_PIN3, getEncoderTickNumber, RISING);
attachInterrupt(INT_PIN4, getEncoderTickNumber, RISING);
The function getEncoderTickNumber() cannot be the interrupt handler because it blocks with a while(1) loop - it will quickly trigger the watchdog and reboot. Additionally, you've already used this function as a task which runs in the background (and seems to expect input from the interrupt handlers).
Finally, you seem have a more suitable candidate for the position of an interrupt handler - the function io_expander_interrupt() - which currently doesn't do anything useful. Let's fix that.
You would need 4 interrupt handlers, one per each GPIO you're monitoring. Each handler is attached to its respective GPIO pin, triggers when the IO rises and each does its own encoder calculation. A simple implementation without extra tasks would look like this:
#include <Arduino.h>
// Motor encoder output pulse per rotation (AndyMark Neverest 60)
int ENC_COUNT_REV = 420;
// Pulse count from encoder. Must be volatile as it's shared between ISR and main task
volatile int encoderValueA1 = 0;
volatile int encoderValueA2 = 0;
volatile int encoderValueA3 = 0;
volatile int encoderValueA4 = 0;
#define INT_PIN1 17
#define INT_PIN2 18
#define INT_PIN3 19
#define INT_PIN4 16
void isr_rising_gpio1() {
encoderValueA1++
}
void isr_rising_gpio2() {
encoderValueA2++
}
void isr_rising_gpio3() {
encoderValueA3++
}
void isr_rising_gpio4() {
encoderValueA4++
}
void setup()
{
Serial.begin(115200);
pinMode(INT_PIN1, INPUT);
attachInterrupt(INT_PIN1, isr_rising_gpio1, RISING);
pinMode(INT_PIN2, INPUT);
attachInterrupt(INT_PIN2, isr_rising_gpio2, RISING);
pinMode(INT_PIN3, INPUT);
attachInterrupt(INT_PIN3, isr_rising_gpio3, RISING);
pinMode(INT_PIN4, INPUT);
attachInterrupt(INT_PIN4, isr_rising_gpio4, RISING);
}
void loop()
{
int rpm1 = (encoderValueA1 * 60) / ENC_COUNT_REV;
encoderValueA1 = 0;
int rpm2 = (encoderValueA2 * 60) / ENC_COUNT_REV;
encoderValueA2 = 0;
int rpm3 = (encoderValueA3 * 60) / ENC_COUNT_REV;
encoderValueA3 = 0;
int rpm4 = (encoderValueA4 * 60) / ENC_COUNT_REV;
encoderValueA4 = 0;
Serial.print("1:");
Serial.print(rpm1);
Serial.print(" 2:");
Serial.print(rpm2);
Serial.print(" 3:");
Serial.print(rpm3);
Serial.print(" 4:");
Serial.println(rpm4);
vTaskDelay(pdMS_TO_TICKS(1000));
}