esp32の内蔵パルスカウンタを使った2相式ロータリーエンコーダの関数を公開する
#include "driver/pcnt.h"
void qei_setup_x4(pcnt_unit_t pcnt_unit, int gpioA, int gpioB)
{
pcnt_config_t pcnt_confA;
pcnt_config_t pcnt_confB;
pcnt_confA.unit = pcnt_unit;
pcnt_confA.channel = PCNT_CHANNEL_0;
pcnt_confA.pulse_gpio_num = gpioA;
pcnt_confA.ctrl_gpio_num = gpioB;
pcnt_confA.pos_mode = PCNT_COUNT_INC;
pcnt_confA.neg_mode = PCNT_COUNT_DEC;
pcnt_confA.lctrl_mode = PCNT_MODE_REVERSE;
pcnt_confA.hctrl_mode = PCNT_MODE_KEEP;
pcnt_confA.counter_h_lim = 32767;
pcnt_confA.counter_l_lim = -32768;
pcnt_confB.unit = pcnt_unit;
pcnt_confB.channel = PCNT_CHANNEL_1;
pcnt_confB.pulse_gpio_num = gpioB;
pcnt_confB.ctrl_gpio_num = gpioA;
pcnt_confB.pos_mode = PCNT_COUNT_INC;
pcnt_confB.neg_mode = PCNT_COUNT_DEC;
pcnt_confB.lctrl_mode = PCNT_MODE_KEEP;
pcnt_confB.hctrl_mode = PCNT_MODE_REVERSE;
pcnt_confB.counter_h_lim = 32767;
pcnt_confB.counter_l_lim = -32768;
/* Initialize PCNT unit */
pcnt_unit_config(&pcnt_confA);
pcnt_unit_config(&pcnt_confB);
pcnt_counter_pause(pcnt_unit);
pcnt_counter_clear(pcnt_unit);
pcnt_counter_resume(pcnt_unit);
}
void qei_setup_x1(pcnt_unit_t pcnt_unit, int gpioA, int gpioB)
{
pcnt_config_t pcnt_confA;
pcnt_confA.unit = pcnt_unit;
pcnt_confA.channel = PCNT_CHANNEL_0;
pcnt_confA.pulse_gpio_num = gpioA;
pcnt_confA.ctrl_gpio_num = gpioB;
pcnt_confA.pos_mode = PCNT_COUNT_INC; //立ち上がりのみカウント
pcnt_confA.neg_mode = PCNT_COUNT_DIS; //立ち下がりはカウントしない
pcnt_confA.lctrl_mode = PCNT_MODE_REVERSE; //立ち上がり時にB相がHighなら逆転
pcnt_confA.hctrl_mode = PCNT_MODE_KEEP;
pcnt_confA.counter_h_lim = 32767;//max 32767;
pcnt_confA.counter_l_lim = -32767;//min -32768;
/* Initialize PCNT unit */
pcnt_unit_config(&pcnt_confA);
pcnt_counter_pause(pcnt_unit);
pcnt_counter_clear(pcnt_unit);
pcnt_counter_resume(pcnt_unit);
}
上記をメインのinoファイルと同じ所に配置します。
#include "qei.hpp"
void setup()
{
Serial.begin(115200);
qei_setup_x4(PCNT_UNIT_2, GPIO_NUM_35, GPIO_NUM_34); //LEFT
qei_setup_x4(PCNT_UNIT_3, GPIO_NUM_39, GPIO_NUM_36); //RIGHT
}
void loop()
{
int16_t count_L;
int16_t count_R;
pcnt_get_counter_value(PCNT_UNIT_2, &count_L);
pcnt_get_counter_value(PCNT_UNIT_3, &count_R);
Serial.print(" Pulse L:");
Serial.print(count_L);
Serial.print(" R:");
Serial.print(count_R);
Serial.print(" ");
}
左右のモーターに接続された2つのエンコーダをイメージしてます。
GPIO35,34に左エンコーダのA相、B相を接続、GPIO39,36に右エンコーダA相、B相を接続。
qei_setup_x4(ユニット番号、A相のピン番号、B相のピン番号)
qei_setup_x1(ユニット番号、A相のピン番号、B相のピン番号)
ユニットがパルスカウンタに相当し、PCNT_UNIT_0からPCNT_UNIT_7まで独立した8個のカウンタを持っています。
各ユニットにはA相、B相に相当する2チャンネルが内蔵されているので2相式エンコーダ1個で1個のユニットを専有します。
関数ファイルはここ
esp_car/qei.hpp at master · bleach31/esp_car
面倒でヘッダに実装しているのでちゃんとし直したほうが良いです・・・。
またx2は未実装です。x1x4で事足りたので。
公式リファレンス
Pulse Counter — ESP-IDF Programming Guide v3.2-dev-343-g561884b documentation
割り込み
今回は省略しています。公式リファレンスのサンプルをみると
esp-idf/pcnt_example_main.c at 561884b62fac66fab29a8c9619556ddd27577d7b · espressif/esp-idf
以下のようにイベントをEnableしてハンドラを登録するみたいです。
/* Set threshold 0 and 1 values and enable events to watch */
pcnt_set_event_value(PCNT_TEST_UNIT, PCNT_EVT_THRES_1, PCNT_THRESH1_VAL);
pcnt_event_enable(PCNT_TEST_UNIT, PCNT_EVT_THRES_1);
pcnt_set_event_value(PCNT_TEST_UNIT, PCNT_EVT_THRES_0, PCNT_THRESH0_VAL);
pcnt_event_enable(PCNT_TEST_UNIT, PCNT_EVT_THRES_0);
/* Enable events on zero, maximum and minimum limit values */
pcnt_event_enable(PCNT_TEST_UNIT, PCNT_EVT_ZERO);
pcnt_event_enable(PCNT_TEST_UNIT, PCNT_EVT_H_LIM);
pcnt_event_enable(PCNT_TEST_UNIT, PCNT_EVT_L_LIM);
/* Initialize PCNT's counter */
pcnt_counter_pause(PCNT_TEST_UNIT);
pcnt_counter_clear(PCNT_TEST_UNIT);
/* Register ISR handler and enable interrupts for PCNT unit */
pcnt_isr_register(pcnt_example_intr_handler, NULL, 0, &user_isr_handle);
pcnt_intr_enable(PCNT_TEST_UNIT);
ハンドラーはこんな感じ。
static void IRAM_ATTR pcnt_example_intr_handler(void *arg)
{
uint32_t intr_status = PCNT.int_st.val;
int i;
pcnt_evt_t evt;
portBASE_TYPE HPTaskAwoken = pdFALSE;
for (i = 0; i < PCNT_UNIT_MAX; i++) {
if (intr_status & (BIT(i))) {
evt.unit = i;
/* Save the PCNT event type that caused an interrupt
to pass it to the main program */
evt.status = PCNT.status_unit[i].val;
PCNT.int_clr.val = BIT(i);
xQueueSendFromISR(pcnt_evt_queue, &evt, &HPTaskAwoken);
if (HPTaskAwoken == pdTRUE) {
portYIELD_FROM_ISR();
}
}
}
}
x4,x1の違いについてはこちらを参考
クワドラチャエンコーダ測定の理論と実測 - National Instruments