Skip to content

Commit ea0b769

Browse files
runger1101001runger1101001
authored
Merge pull request #44 from mcells/Esp32HwEncoder-feature
Add ESP32 Hardware Encoder Class
2 parents 52b754d + 855e110 commit ea0b769

File tree

3 files changed

+313
-0
lines changed

3 files changed

+313
-0
lines changed

‎src/encoders/esp32hwencoder/ESP32HWEncoder.cpp‎

Lines changed: 198 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,198 @@
1+
#include "ESP32HWEncoder.h"
2+
3+
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
4+
5+
6+
7+
ESP32HWEncoder::ESP32HWEncoder(int pinA, int pinB, int32_t ppr, int pinI)
8+
{
9+
#ifdef USE_ARDUINO_PINOUT
10+
// Handle Arduino Nano ESP32 quirks with the pin assignments
11+
_pinA = digitalPinToGPIO(pinA);
12+
_pinB = digitalPinToGPIO(pinB);
13+
_pinI = digitalPinToGPIO(pinI);
14+
#else
15+
_pinA = pinA;
16+
_pinB = pinB;
17+
_pinI = pinI;
18+
#endif
19+
20+
cpr = ppr * 4; // 4x for quadrature
21+
22+
pcnt_config.ctrl_gpio_num = _pinA;
23+
pcnt_config.pulse_gpio_num = _pinB;
24+
pcnt_config.counter_l_lim = INT16_MIN;
25+
pcnt_config.counter_h_lim = INT16_MAX;
26+
}
27+
28+
// Interrupt handler for accumulating the pulsecounter count
29+
void IRAM_ATTR overflowCounter(void* arg)
30+
{
31+
uint8_t interruptStatus = PCNT.int_st.val;
32+
for (uint32_t i = 0; i < 8; i++)
33+
{
34+
if (interruptStatus & BIT(i))
35+
{
36+
int32_t set = ((overflowISR_args_t*) arg)[i].set;
37+
if(set != 1){ continue;}
38+
int32_t* count = ((overflowISR_args_t*) arg)[i].angleoverflow_val;
39+
40+
// Add or subtract depending on the direction of the overflow
41+
switch (PCNT.status_unit[i].val)
42+
{
43+
case PCNT_EVT_L_LIM:
44+
*count += INT16_MIN;
45+
break;
46+
case PCNT_EVT_H_LIM:
47+
*count += INT16_MAX;
48+
break;
49+
default:
50+
break;
51+
}
52+
53+
// Clear the interrupt
54+
PCNT.int_clr.val = BIT(i);
55+
}
56+
}
57+
}
58+
59+
// Interrupt handler for zeroing the pulsecounter count
60+
void IRAM_ATTR ESP32HWEncoder::indexHandler()
61+
{
62+
pcnt_counter_clear(pcnt_config.unit);
63+
angleOverflow = 0;
64+
indexFound = true;
65+
}
66+
67+
void ESP32HWEncoder::init()
68+
{
69+
70+
// Statically allocate and initialize the spinlock
71+
spinlock = portMUX_INITIALIZER_UNLOCKED;
72+
73+
// find a free pulsecount unit
74+
for (uint8_t i = 0; i < PCNT_UNIT_MAX; i++)
75+
{
76+
if(cpr > 0){
77+
inv_cpr = 1.0f/cpr;
78+
}
79+
if(used_units[i] == 0){
80+
pcnt_config.unit = (pcnt_unit_t) i;
81+
if(pcnt_unit_config(&pcnt_config) == ESP_OK){
82+
initialized = true;
83+
// Serial.printf("Configured PCNT unit %d\n", i);
84+
used_units[i] = 1;
85+
break;
86+
}
87+
}
88+
89+
}
90+
if (initialized)
91+
{
92+
// Set up the PCNT peripheral
93+
pcnt_set_pin(pcnt_config.unit, PCNT_CHANNEL_0, pcnt_config.ctrl_gpio_num, pcnt_config.pulse_gpio_num);
94+
pcnt_set_pin(pcnt_config.unit, PCNT_CHANNEL_1, pcnt_config.pulse_gpio_num, pcnt_config.ctrl_gpio_num);
95+
pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_0, PCNT_COUNT_INC, PCNT_COUNT_DEC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP);
96+
pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_1, PCNT_COUNT_DEC, PCNT_COUNT_INC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP);
97+
98+
pcnt_counter_pause(pcnt_config.unit);
99+
pcnt_counter_clear(pcnt_config.unit);
100+
101+
// Select interrupt on reaching high and low counter limit
102+
pcnt_event_enable(pcnt_config.unit, PCNT_EVT_L_LIM);
103+
pcnt_event_enable(pcnt_config.unit, PCNT_EVT_H_LIM);
104+
105+
// Pass pointer to the angle accumulator and the current PCNT unit to the ISR
106+
overflowISR_args[pcnt_config.unit].angleoverflow_val = &angleOverflow;
107+
overflowISR_args[pcnt_config.unit].unit = pcnt_config.unit;
108+
overflowISR_args[pcnt_config.unit].set = 1;
109+
110+
// Register and enable the interrupt
111+
pcnt_isr_register(overflowCounter, (void*)&overflowISR_args, 0, (pcnt_isr_handle_t*) NULL);
112+
pcnt_intr_enable(pcnt_config.unit);
113+
114+
// Just check the last command for errors
115+
if(pcnt_counter_resume(pcnt_config.unit) != ESP_OK){
116+
initialized = false;
117+
}
118+
119+
// If an index Pin is defined, create an ISR to zero the angle when the index fires
120+
if (hasIndex())
121+
{
122+
attachInterrupt(static_cast<u_int8_t>(_pinI), std::bind(&ESP32HWEncoder::indexHandler,this), RISING);
123+
}
124+
125+
// Optionally use pullups
126+
if (pullup == USE_INTERN)
127+
{
128+
pinMode(static_cast<u_int8_t>(_pinA), INPUT_PULLUP);
129+
pinMode(static_cast<u_int8_t>(_pinB), INPUT_PULLUP);
130+
if (hasIndex()) {pinMode(static_cast<u_int8_t>(_pinI), INPUT_PULLUP);}
131+
}
132+
133+
}
134+
135+
}
136+
137+
int ESP32HWEncoder::needsSearch()
138+
{
139+
return !((indexFound && hasIndex()) || !hasIndex());
140+
}
141+
142+
int ESP32HWEncoder::hasIndex()
143+
{
144+
return _pinI != -1;
145+
}
146+
147+
void ESP32HWEncoder::setCpr(int32_t ppr){
148+
cpr = 4*ppr;
149+
if(cpr > 0){
150+
inv_cpr = 1.0f/cpr; // Precalculate the inverse of cpr to avoid "slow" float divisions
151+
}
152+
}
153+
154+
int32_t ESP32HWEncoder::getCpr(){
155+
return cpr;
156+
}
157+
158+
// Change to Step/Dir counting mode. A->Step, B->Dir
159+
void ESP32HWEncoder::setStepDirMode(){
160+
pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_0, PCNT_COUNT_INC, PCNT_COUNT_DIS, PCNT_MODE_KEEP, PCNT_MODE_KEEP);
161+
pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_1, PCNT_COUNT_DIS, PCNT_COUNT_DIS, PCNT_MODE_KEEP, PCNT_MODE_REVERSE);
162+
}
163+
164+
// Change to default AB (quadrature) mode
165+
void ESP32HWEncoder::setQuadratureMode(){
166+
pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_0, PCNT_COUNT_INC, PCNT_COUNT_DEC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP);
167+
pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_1, PCNT_COUNT_DEC, PCNT_COUNT_INC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP);
168+
}
169+
170+
float IRAM_ATTR ESP32HWEncoder::getSensorAngle()
171+
{
172+
if(!initialized){return -1.0f;}
173+
174+
taskENTER_CRITICAL(&spinlock);
175+
// We are now in a critical section to prevent interrupts messing with angleOverflow and angleCounter
176+
177+
// Retrieve the count register into a variable
178+
pcnt_get_counter_value(pcnt_config.unit, &angleCounter);
179+
180+
// Trim the accumulator variable to prevent issues with it overflowing
181+
// Make the % operand behave mathematically correct (-5 modulo 4 == 3; -5 % 4 == -1)
182+
angleOverflow %= cpr;
183+
if (angleOverflow < 0){
184+
angleOverflow += cpr;
185+
}
186+
187+
angleSum = (angleOverflow + angleCounter) % cpr;
188+
if (angleSum < 0) {
189+
angleSum += cpr;
190+
}
191+
192+
taskEXIT_CRITICAL(&spinlock); // Exit critical section
193+
194+
// Calculate the shaft angle
195+
return _2PI * angleSum * inv_cpr;
196+
}
197+
198+
#endif

‎src/encoders/esp32hwencoder/ESP32HWEncoder.h‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
2+
#pragma once
3+
4+
#include <Arduino.h>
5+
6+
7+
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
8+
9+
#include "driver/pcnt.h"
10+
#include "soc/pcnt_struct.h"
11+
#include "common/base_classes/Sensor.h"
12+
#include "common/foc_utils.h"
13+
#include "FunctionalInterrupt.h"
14+
15+
static struct overflowISR_args_t {
16+
int32_t* angleoverflow_val;
17+
pcnt_unit_t unit;
18+
int32_t set;
19+
}overflowISR_args[PCNT_UNIT_MAX];
20+
21+
// Statically allocate and initialize a spinlock
22+
static portMUX_TYPE spinlock;
23+
static int used_units[PCNT_UNIT_MAX];
24+
25+
class ESP32HWEncoder : public Sensor{
26+
public:
27+
/**
28+
Encoder class constructor
29+
@param ppr impulses per rotation (cpr=ppr*4)
30+
*/
31+
explicit ESP32HWEncoder(int pinA, int pinB, int32_t ppr, int pinI=-1);
32+
33+
void init() override;
34+
int needsSearch() override;
35+
int hasIndex();
36+
float getSensorAngle() override;
37+
void setCpr(int32_t ppr);
38+
int32_t getCpr();
39+
void setStepDirMode();
40+
void setQuadratureMode();
41+
bool initialized = false;
42+
43+
Pullup pullup; //!< Configuration parameter internal or external pullups
44+
45+
46+
47+
protected:
48+
49+
50+
void IRAM_ATTR indexHandler();
51+
52+
bool indexFound = false;
53+
54+
int _pinA, _pinB, _pinI;
55+
56+
pcnt_config_t pcnt_config;
57+
58+
int16_t angleCounter; // Stores the PCNT value
59+
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.
60+
int32_t angleSum; // Sum of Counter and Overflow in range [0,cpr]
61+
62+
int32_t cpr; // Counts per rotation = 4 * ppr for quadrature encoders
63+
float inv_cpr;
64+
};
65+
66+
#endif

‎src/encoders/esp32hwencoder/README.md‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
# SimpleFOC Driver for ESP32 hardware encoder
2+
3+
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.
4+
5+
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.
6+
The PCNT peripheral can count at several MHz and should not be a limiting factor here.
7+
8+
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 ;-) )
9+
10+
11+
## Status
12+
13+
Seems to work fine! Step/Dir mode is untested.
14+
15+
## Hardware Setup
16+
17+
You can connect the encoder to any digital input pin of the ESP32, as they all support the PCNT peripheral.
18+
19+
## Configuration
20+
21+
This is a near drop-in replacement for the standard encoder class:
22+
23+
```c++
24+
#include "Arduino.h"
25+
#include "SimpleFOC.h"
26+
#include "SimpleFOCDrivers.h"
27+
#include "encoders/esp32hwencoder/ESP32HWEncoder.h"
28+
29+
#define ENCODER_PPR 4711
30+
#define ENCODER_PIN_A 16
31+
#define ENCODER_PIN_B 17
32+
#define ENCODER_PIN_I 21
33+
34+
ESP32HWEncoder encoder = ESP32HWEncoder(ENCODER_PIN_A, ENCODER_PIN_B, ENCODER_PPR, ENCODER_PIN_I); // The Index pin can be omitted
35+
36+
void setup() {
37+
encoder.pullup = Pullup::USE_INTERN; // optional: pullups
38+
39+
encoder.setStepDirMode(); // optional: set Stepper type step/dir mode
40+
41+
encoder.init();
42+
}
43+
44+
void loop() {
45+
encoder.update(); // optional: Update manually if not using loopfoc()
46+
47+
encoder.getAngle() // Access the sensor value
48+
}
49+
```

0 commit comments

Comments
 (0)