ESPNowESK8/src/rpm.h

#define halBlueLeft 2
#define halYellowLeft 22
#define halGreenLeft 19

#define halBlueRight 23
#define halYellowRight 18
#define halGreenRight 5

#define FORWARD 0
#define BACKWARD 1

#define wheelCircumference 26.062
#define pulsesPerRotation 22

#include "driver/pcnt.h"

TaskHandle_t rpmTaskHandle;
float rpmRight = 0;
float rpmLeft = 0;

int lastState[2] = {1, 1};
int16_t counter[2] = {0, 0};
int direction[2] = {0, 0};

long rpmTimer = 0;

// Zero states are non existant
int motorPosition[2][2][2]{
    {{0, 1},
     {3, 2}},
    {{5, 6},
     {4, 0}}};
// old state, new state
int relativeLookup[6][6]{
    {0, 1, 2, 3, -2, -1},  // 1
    {-1, 0, 1, 2, 3, -1},  // 2
    {-2, -1, 0, 1, 2, 3},  // 3
    {-3, -2, -1, 0, 1, 2}, // 4
    {1, -3, -2, -1, 0, 1}, // 5
    {1, 2, -3, -2, -1, 0}  // 6
};

#define PCNT_UNIT_LEFT PCNT_UNIT_0
#define PCNT_UNIT_RIGHT PCNT_UNIT_1

int getState(bool isLeft)
{
  if (isLeft)
  {
    return motorPosition[digitalRead(halBlueLeft)][digitalRead(halYellowLeft)][digitalRead(halGreenLeft)];
  }
  else
  {
    return motorPosition[digitalRead(halBlueRight)][digitalRead(halYellowRight)][digitalRead(halGreenRight)];
  }
}

void initRPMPins()
{
  pinMode(halBlueLeft, INPUT_PULLUP);
  pinMode(halYellowLeft, INPUT_PULLUP);
  pinMode(halGreenLeft, INPUT_PULLUP);

  pinMode(halBlueRight, INPUT_PULLUP);
  pinMode(halYellowRight, INPUT_PULLUP);
  pinMode(halGreenRight, INPUT_PULLUP);

  // init counter
  pcnt_config_t pcnt_config_left = {
      .pulse_gpio_num = halBlueLeft,
      .ctrl_gpio_num = PCNT_PIN_NOT_USED,
      .lctrl_mode = PCNT_MODE_KEEP,
      .hctrl_mode = PCNT_MODE_KEEP,
      .pos_mode = PCNT_COUNT_INC, // count both rising and falling edges
      .neg_mode = PCNT_COUNT_INC,
      .counter_h_lim = 32767,
      .counter_l_lim = -1,
      .unit = PCNT_UNIT_LEFT,
      .channel = PCNT_CHANNEL_0};

  pcnt_config_t pcnt_config_right = {
      .pulse_gpio_num = halBlueRight,
      .ctrl_gpio_num = PCNT_PIN_NOT_USED,
      .lctrl_mode = PCNT_MODE_KEEP,
      .hctrl_mode = PCNT_MODE_KEEP,
      .pos_mode = PCNT_COUNT_INC, // count both rising and falling edges
      .neg_mode = PCNT_COUNT_INC,
      .counter_h_lim = 32767,
      .counter_l_lim = -1,
      .unit = PCNT_UNIT_RIGHT,
      .channel = PCNT_CHANNEL_0};

  pcnt_unit_config(&pcnt_config_left);
  pcnt_unit_config(&pcnt_config_right);

  pcnt_counter_pause(PCNT_UNIT_LEFT);
  pcnt_counter_pause(PCNT_UNIT_RIGHT);

  pcnt_counter_clear(PCNT_UNIT_LEFT);
  pcnt_counter_clear(PCNT_UNIT_RIGHT);

  pcnt_counter_resume(PCNT_UNIT_LEFT);
  pcnt_counter_resume(PCNT_UNIT_RIGHT);
}

void measureRpm(void *parameter)
{

  while (true)
  {
    if (millis() - rpmTimer > 250)
    {
      // read counters
      pcnt_get_counter_value(PCNT_UNIT_LEFT, &counter[1]);
      pcnt_get_counter_value(PCNT_UNIT_RIGHT, &counter[0]);

      pcnt_counter_clear(PCNT_UNIT_LEFT);
      pcnt_counter_clear(PCNT_UNIT_RIGHT);

      rpmRight = (counter[0] / pulsesPerRotation) * 4; // LB: Actually only rps
      rpmLeft = (counter[1] / pulsesPerRotation) * 4;
      kmh = (wheelCircumference / 100) * (rpmRight + rpmLeft) / 2 * 3600 / 1000;
      // Serial.print(kmh); Serial.print(" "); Serial.println(direction[0]);
      sendSpeed = abs(floor(kmh));
      sendSpeedDecimals = (kmh - sendSpeed) * 100;
      counter[true] = 0;
      counter[false] = 0;
      rpmTimer = millis();
    }

    int state = getState(0);
    if (lastState[0] != state)
    {
      if (relativeLookup[lastState[0] - 1][state - 1] > 0)
      {
        direction[0] = FORWARD;
      }
      else
      {
        direction[0] = BACKWARD;
      }
      lastState[0] = state;
    }

    state = getState(1);
    if (lastState[1] != state)
    {
      if (relativeLookup[lastState[1] - 1][state - 1] > 0)
      {
        direction[1] = FORWARD;
      }
      else
      {
        direction[1] = BACKWARD;
      }

      lastState[1] = state;
    }

    vTaskDelay(1 / portTICK_PERIOD_MS);
  }
}