diff --git a/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp b/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp new file mode 100644 index 0000000..29fcb9c --- /dev/null +++ b/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp @@ -0,0 +1,198 @@ +#include "ESP32HWEncoder.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + + + +ESP32HWEncoder::ESP32HWEncoder(int pinA, int pinB, int32_t ppr, int pinI) +{ + #ifdef USE_ARDUINO_PINOUT + // Handle Arduino Nano ESP32 quirks with the pin assignments + _pinA = digitalPinToGPIO(pinA); + _pinB = digitalPinToGPIO(pinB); + _pinI = digitalPinToGPIO(pinI); + #else + _pinA = pinA; + _pinB = pinB; + _pinI = pinI; + #endif + + cpr = ppr * 4; // 4x for quadrature + + pcnt_config.ctrl_gpio_num = _pinA; + pcnt_config.pulse_gpio_num = _pinB; + pcnt_config.counter_l_lim = INT16_MIN; + pcnt_config.counter_h_lim = INT16_MAX; +} + +// Interrupt handler for accumulating the pulsecounter count +void IRAM_ATTR overflowCounter(void* arg) +{ + uint8_t interruptStatus = PCNT.int_st.val; + for (uint32_t i = 0; i < 8; i++) + { + if (interruptStatus & BIT(i)) + { + int32_t set = ((overflowISR_args_t*) arg)[i].set; + if(set != 1){ continue;} + int32_t* count = ((overflowISR_args_t*) arg)[i].angleoverflow_val; + + // Add or subtract depending on the direction of the overflow + switch (PCNT.status_unit[i].val) + { + case PCNT_EVT_L_LIM: + *count += INT16_MIN; + break; + case PCNT_EVT_H_LIM: + *count += INT16_MAX; + break; + default: + break; + } + + // Clear the interrupt + PCNT.int_clr.val = BIT(i); + } + } +} + +// Interrupt handler for zeroing the pulsecounter count +void IRAM_ATTR ESP32HWEncoder::indexHandler() + { + pcnt_counter_clear(pcnt_config.unit); + angleOverflow = 0; + indexFound = true; + } + +void ESP32HWEncoder::init() +{ + + // Statically allocate and initialize the spinlock + spinlock = portMUX_INITIALIZER_UNLOCKED; + + // find a free pulsecount unit + for (uint8_t i = 0; i < PCNT_UNIT_MAX; i++) + { + if(cpr > 0){ + inv_cpr = 1.0f/cpr; + } + if(used_units[i] == 0){ + pcnt_config.unit = (pcnt_unit_t) i; + if(pcnt_unit_config(&pcnt_config) == ESP_OK){ + initialized = true; + // Serial.printf("Configured PCNT unit %d\n", i); + used_units[i] = 1; + break; + } + } + + } + if (initialized) + { + // Set up the PCNT peripheral + pcnt_set_pin(pcnt_config.unit, PCNT_CHANNEL_0, pcnt_config.ctrl_gpio_num, pcnt_config.pulse_gpio_num); + pcnt_set_pin(pcnt_config.unit, PCNT_CHANNEL_1, pcnt_config.pulse_gpio_num, pcnt_config.ctrl_gpio_num); + pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_0, PCNT_COUNT_INC, PCNT_COUNT_DEC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP); + pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_1, PCNT_COUNT_DEC, PCNT_COUNT_INC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP); + + pcnt_counter_pause(pcnt_config.unit); + pcnt_counter_clear(pcnt_config.unit); + + // Select interrupt on reaching high and low counter limit + pcnt_event_enable(pcnt_config.unit, PCNT_EVT_L_LIM); + pcnt_event_enable(pcnt_config.unit, PCNT_EVT_H_LIM); + + // Pass pointer to the angle accumulator and the current PCNT unit to the ISR + overflowISR_args[pcnt_config.unit].angleoverflow_val = &angleOverflow; + overflowISR_args[pcnt_config.unit].unit = pcnt_config.unit; + overflowISR_args[pcnt_config.unit].set = 1; + + // Register and enable the interrupt + pcnt_isr_register(overflowCounter, (void*)&overflowISR_args, 0, (pcnt_isr_handle_t*) NULL); + pcnt_intr_enable(pcnt_config.unit); + + // Just check the last command for errors + if(pcnt_counter_resume(pcnt_config.unit) != ESP_OK){ + initialized = false; + } + + // If an index Pin is defined, create an ISR to zero the angle when the index fires + if (hasIndex()) + { + attachInterrupt(static_cast(_pinI), std::bind(&ESP32HWEncoder::indexHandler,this), RISING); + } + + // Optionally use pullups + if (pullup == USE_INTERN) + { + pinMode(static_cast(_pinA), INPUT_PULLUP); + pinMode(static_cast(_pinB), INPUT_PULLUP); + if (hasIndex()) {pinMode(static_cast(_pinI), INPUT_PULLUP);} + } + + } + +} + +int ESP32HWEncoder::needsSearch() +{ + return !((indexFound && hasIndex()) || !hasIndex()); +} + +int ESP32HWEncoder::hasIndex() +{ + return _pinI != -1; +} + +void ESP32HWEncoder::setCpr(int32_t ppr){ + cpr = 4*ppr; + if(cpr > 0){ + inv_cpr = 1.0f/cpr; // Precalculate the inverse of cpr to avoid "slow" float divisions + } +} + +int32_t ESP32HWEncoder::getCpr(){ + return cpr; +} + +// Change to Step/Dir counting mode. A->Step, B->Dir +void ESP32HWEncoder::setStepDirMode(){ + pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_0, PCNT_COUNT_INC, PCNT_COUNT_DIS, PCNT_MODE_KEEP, PCNT_MODE_KEEP); + pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_1, PCNT_COUNT_DIS, PCNT_COUNT_DIS, PCNT_MODE_KEEP, PCNT_MODE_REVERSE); +} + +// Change to default AB (quadrature) mode +void ESP32HWEncoder::setQuadratureMode(){ + pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_0, PCNT_COUNT_INC, PCNT_COUNT_DEC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP); + pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_1, PCNT_COUNT_DEC, PCNT_COUNT_INC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP); +} + +float IRAM_ATTR ESP32HWEncoder::getSensorAngle() +{ + if(!initialized){return -1.0f;} + + taskENTER_CRITICAL(&spinlock); + // We are now in a critical section to prevent interrupts messing with angleOverflow and angleCounter + + // Retrieve the count register into a variable + pcnt_get_counter_value(pcnt_config.unit, &angleCounter); + + // Trim the accumulator variable to prevent issues with it overflowing + // Make the % operand behave mathematically correct (-5 modulo 4 == 3; -5 % 4 == -1) + angleOverflow %= cpr; + if (angleOverflow < 0){ + angleOverflow += cpr; + } + + angleSum = (angleOverflow + angleCounter) % cpr; + if (angleSum < 0) { + angleSum += cpr; + } + + taskEXIT_CRITICAL(&spinlock); // Exit critical section + + // Calculate the shaft angle + return _2PI * angleSum * inv_cpr; +} + +#endif \ No newline at end of file diff --git a/src/encoders/esp32hwencoder/ESP32HWEncoder.h b/src/encoders/esp32hwencoder/ESP32HWEncoder.h new file mode 100644 index 0000000..ad3a42f --- /dev/null +++ b/src/encoders/esp32hwencoder/ESP32HWEncoder.h @@ -0,0 +1,66 @@ + +#pragma once + +#include + + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + +#include "driver/pcnt.h" +#include "soc/pcnt_struct.h" +#include "common/base_classes/Sensor.h" +#include "common/foc_utils.h" +#include "FunctionalInterrupt.h" + +static struct overflowISR_args_t { + int32_t* angleoverflow_val; + pcnt_unit_t unit; + int32_t set; +}overflowISR_args[PCNT_UNIT_MAX]; + +// Statically allocate and initialize a spinlock +static portMUX_TYPE spinlock; +static int used_units[PCNT_UNIT_MAX]; + +class ESP32HWEncoder : public Sensor{ + public: + /** + Encoder class constructor + @param ppr impulses per rotation (cpr=ppr*4) + */ + explicit ESP32HWEncoder(int pinA, int pinB, int32_t ppr, int pinI=-1); + + void init() override; + int needsSearch() override; + int hasIndex(); + float getSensorAngle() override; + void setCpr(int32_t ppr); + int32_t getCpr(); + void setStepDirMode(); + void setQuadratureMode(); + bool initialized = false; + + Pullup pullup; //!< Configuration parameter internal or external pullups + + + + protected: + + + void IRAM_ATTR indexHandler(); + + bool indexFound = false; + + int _pinA, _pinB, _pinI; + + pcnt_config_t pcnt_config; + + int16_t angleCounter; // Stores the PCNT value + int32_t angleOverflow; // In case the PCNT peripheral overflows, this accumulates the max count to keep track of large counts/angles (>= 16 Bit). On index, this gets reset. + int32_t angleSum; // Sum of Counter and Overflow in range [0,cpr] + + int32_t cpr; // Counts per rotation = 4 * ppr for quadrature encoders + float inv_cpr; +}; + +#endif \ No newline at end of file diff --git a/src/encoders/esp32hwencoder/README.md b/src/encoders/esp32hwencoder/README.md new file mode 100644 index 0000000..c6a7033 --- /dev/null +++ b/src/encoders/esp32hwencoder/README.md @@ -0,0 +1,49 @@ +# SimpleFOC Driver for ESP32 hardware encoder + +This encoder driver uses the ESP32´s dedicated pulse counter hardware to efficiently count the AB(I) signals of an encoder. It also supports a Step/Dir-type mode. + +Because most of the counting is done by the peripheral, it should support much higher speeds in comparison to the generic interrupt-based encoder implementation provided in the main library. +The PCNT peripheral can count at several MHz and should not be a limiting factor here. + +You can use encoders with cpr of up to 31 bits. (At this resolution, you would get about 100 counts per second if you mounted such a sensor on the earths rotational axis. Thats plenty ;-) ) + + +## Status + +Seems to work fine! Step/Dir mode is untested. + +## Hardware Setup + +You can connect the encoder to any digital input pin of the ESP32, as they all support the PCNT peripheral. + +## Configuration + +This is a near drop-in replacement for the standard encoder class: + +```c++ +#include "Arduino.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "encoders/esp32hwencoder/ESP32HWEncoder.h" + +#define ENCODER_PPR 4711 +#define ENCODER_PIN_A 16 +#define ENCODER_PIN_B 17 +#define ENCODER_PIN_I 21 + +ESP32HWEncoder encoder = ESP32HWEncoder(ENCODER_PIN_A, ENCODER_PIN_B, ENCODER_PPR, ENCODER_PIN_I); // The Index pin can be omitted + +void setup() { + encoder.pullup = Pullup::USE_INTERN; // optional: pullups + + encoder.setStepDirMode(); // optional: set Stepper type step/dir mode + + encoder.init(); +} + +void loop() { + encoder.update(); // optional: Update manually if not using loopfoc() + + encoder.getAngle() // Access the sensor value +} +``` \ No newline at end of file