Created
September 9, 2024 06:04
-
-
Save melehin/b9651a29fcc7b8d4e3a808ec7c58a151 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| /* | |
| Linuxcnc ModbusRTU Encoder for ESP32 S2 | |
| (c)2024 Fedor Melekhin | |
| Use linuxcnc part (linuxcnc folder and config from README) from https://github.com/futurelink/linuxcnc-spindle-encoder | |
| This code is licensed under the BSD New License | |
| */ | |
| #include "driver/pcnt.h" | |
| #include <ModbusRTU.h> | |
| // heartbeat,pos,pos-in,speed-rpm,index,direction | |
| #define REGHEARTBEAT 10 | |
| #define REGPOS 11 | |
| #define REGPOSIN 12 | |
| #define REGVEL 13 | |
| #define REGINDEX 14 | |
| #define REGDIR 15 | |
| #define REGINDEXENABLE 16 | |
| #define PULSES_PER_REVOLUTION 953 // PULSES per revlution | |
| #define SLAVE_ID 1 | |
| // Define encoder pins | |
| #define ENCODER_PIN_A 16 // Encoder A pin | |
| #define ENCODER_PIN_B 18 // Encoder B pin | |
| #define ENCODER_PIN_Z 33 // Encoder Z pin | |
| int16_t encoderPositionForVelovity = 0; // Encoder position for velocity | |
| int16_t encoderPosition = 0; // Encoder position | |
| unsigned long lastTime = 0; // Last measurement time | |
| uint16_t encDirection = 0; | |
| uint16_t encIndex = 0; | |
| bool encIndexEnable = false; | |
| ModbusRTU mb; | |
| IRAM_ATTR void updateIndex() { | |
| if(encIndexEnable){ | |
| pcnt_counter_pause(PCNT_UNIT_0); | |
| pcnt_counter_clear(PCNT_UNIT_0); | |
| pcnt_counter_resume(PCNT_UNIT_0); | |
| lastTime = millis(); | |
| encIndexEnable = false; | |
| } | |
| encIndex = 1; | |
| encoderPosition = 0; | |
| } | |
| uint16_t heartbeat = 0; | |
| uint16_t cbHeartBeat(TRegister* reg, uint16_t val) { | |
| heartbeat = !heartbeat; | |
| return heartbeat; | |
| } | |
| uint16_t cbPosition(TRegister* reg, uint16_t val) { | |
| return abs(encoderPosition); | |
| } | |
| uint16_t cbPositionIncrement(TRegister* reg, uint16_t val) { | |
| pcnt_get_counter_value(PCNT_UNIT_0, &encoderPositionForVelovity); | |
| pcnt_counter_pause(PCNT_UNIT_0); | |
| pcnt_counter_clear(PCNT_UNIT_0); | |
| pcnt_counter_resume(PCNT_UNIT_0); | |
| encoderPosition += encoderPositionForVelovity; | |
| encDirection = encoderPositionForVelovity < 0 ? 1 : 0; | |
| return abs(encoderPositionForVelovity); | |
| } | |
| uint16_t cbVelocity(TRegister* reg, uint16_t val) { | |
| float diff = millis() - lastTime; | |
| int16_t velocity = (int16_t)(((float)encoderPositionForVelovity / PULSES_PER_REVOLUTION) * (1000.0 / diff) * 60.0); | |
| lastTime = millis(); | |
| return abs(velocity); | |
| } | |
| uint16_t cbIndex(TRegister* reg, uint16_t val) { | |
| uint16_t ind = encIndex; | |
| encIndex = 0; | |
| return ind; | |
| } | |
| uint16_t cbIndexEnable(TRegister* reg, uint16_t val) { | |
| if(val == 1){ | |
| encIndexEnable = true; | |
| } | |
| return val; | |
| } | |
| uint16_t cbDirection(TRegister* reg, uint16_t val) { | |
| return encDirection; | |
| } | |
| void init_encoder(){ | |
| pcnt_config_t r_enc_config; | |
| r_enc_config = { | |
| .pulse_gpio_num = ENCODER_PIN_A, //Rotary Encoder Chan A (GPIO32) | |
| .ctrl_gpio_num = ENCODER_PIN_B, //Rotary Encoder Chan B (GPIO33) | |
| .lctrl_mode = PCNT_MODE_KEEP, // Rising A on HIGH B = CW Step | |
| .hctrl_mode = PCNT_MODE_REVERSE, // Rising A on LOW B = CCW Step | |
| .pos_mode = PCNT_COUNT_INC, //Count Only On Rising-Edges | |
| .neg_mode = PCNT_COUNT_DIS, // Discard Falling-Edge | |
| .counter_h_lim = INT16_MAX, | |
| .counter_l_lim = INT16_MIN, | |
| .unit = PCNT_UNIT_0, | |
| .channel = PCNT_CHANNEL_0, | |
| }; | |
| pcnt_unit_config(&r_enc_config); | |
| pcnt_set_filter_value(PCNT_UNIT_0, 250); // Filter Runt Pulses | |
| pcnt_filter_enable(PCNT_UNIT_0); | |
| pcnt_counter_pause(PCNT_UNIT_0); // Initial PCNT init | |
| pcnt_counter_clear(PCNT_UNIT_0); | |
| pcnt_counter_resume(PCNT_UNIT_0); | |
| } | |
| void setup() { | |
| Serial.begin(38400); | |
| #if defined(ESP32) || defined(ESP8266) | |
| mb.begin(&Serial); | |
| #else | |
| mb.begin(&Serial); | |
| mb.setBaudrate(38400); | |
| #endif | |
| mb.slave(SLAVE_ID); | |
| mb.addHreg(REGHEARTBEAT); | |
| mb.addHreg(REGPOS); | |
| mb.addHreg(REGPOSIN); | |
| mb.addHreg(REGVEL); | |
| mb.addHreg(REGINDEX); | |
| mb.addHreg(REGDIR); | |
| mb.addHreg(REGINDEXENABLE); | |
| mb.onGetHreg(REGHEARTBEAT, cbHeartBeat, 1); | |
| mb.onGetHreg(REGPOS, cbPosition, 1); | |
| mb.onGetHreg(REGPOSIN, cbPositionIncrement, 1); | |
| mb.onGetHreg(REGVEL, cbVelocity, 1); | |
| mb.onGetHreg(REGINDEX, cbIndex, 1); | |
| mb.onGetHreg(REGDIR, cbDirection, 1); | |
| mb.onSetHreg(REGINDEXENABLE, cbIndexEnable, 1); | |
| // Set encoder pins as inputs | |
| pinMode(ENCODER_PIN_A, INPUT); | |
| pinMode(ENCODER_PIN_B, INPUT); | |
| pinMode(ENCODER_PIN_Z, INPUT_PULLUP); | |
| // Attach interrupts for Z Index pin | |
| attachInterrupt(digitalPinToInterrupt(ENCODER_PIN_Z), updateIndex, FALLING); | |
| init_encoder(); | |
| lastTime = millis(); // Initialize last time | |
| } | |
| void loop() { | |
| mb.task(); | |
| yield(); | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment