问如何在FreeRTOS中读取带有中断的电机编码器值？

EN

#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()
{

}