/* This is a hardware step generator with dual core ESP32 and W5500 module * for LinuxCNC * * Copyright 2021 Juhász Zoltán * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ /* Hardware: * * 3v3 -- 3.3v W5500 * GND -- GND W5500 * GND -- (DB15- 7) * GND -- (DB15- 8) * * GPIO 2 -> -[100R]-(DB15 - 9) OUT-00 / onboard blue LED * GPIO 4 -> -[100R]-(DB15 -10) OUT-01 * GPIO 5 -> W5500 SCS * GPIO 12 -> -[100R]-(DB15 - 1) step-0 * GPIO 13 -> -[100R]-(DB15 - 2) dir-0 * (RX2) GPIO 16 -> -[100R]-(DB15 - 3) step-1 * (TX2) GPIO 17 -> -[100R]-(DB15 - 4) dir-1 * GPIO 18 -> W5500 SCLK * GPIO 19 <- W5500 MISO * GPIO 21 -> -[100R]-(DB15 - 5) step-2 * GPIO 22 -> -[100R]-(DB15 - 6) dir-2 * GPIO 23 -> W5500 MOSI * GPIO 25 -> -[100R]-(DB15 -11) OUT-02 * GPIO 26 <- -[100R]-(DB15 -12) IN-00 * GPIO 27 <- -[100R]-(DB15 -13) IN-01 * GPIO 32 <- -[100R]-(DB15 -14) IN-02 * GPIO 33 <- -[100R]-(DB15 -15) IN-03 * GPIO 34 <- NC {no pullup!} * GPIO 35 <- NC {no pullup!} * (VP) GPIO 36 <- NC {no pullup!} * (VN) GPIO 39 <- NC {no pullup!} */ #include #include #include /*==================================================================*/ byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; IPAddress ip(192, 168, 96, 54); unsigned int port = 58427; /*==================================================================*/ #define SPI_CS_PIN 5 /*==================================================================*/ #define STEP_0_PIN 12 #define STEP_0_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT12) #define STEP_0_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT12) #define DIR_0_PIN 13 #define DIR_0_PLUS REG_WRITE(GPIO_OUT_W1TS_REG, BIT13) #define DIR_0_MINUS REG_WRITE(GPIO_OUT_W1TC_REG, BIT13) #define STEP_1_PIN 16 #define STEP_1_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT16) #define STEP_1_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT16) #define DIR_1_PIN 17 #define DIR_1_PLUS REG_WRITE(GPIO_OUT_W1TS_REG, BIT17) #define DIR_1_MINUS REG_WRITE(GPIO_OUT_W1TC_REG, BIT17) #define STEP_2_PIN 21 #define STEP_2_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT21) #define STEP_2_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT21) #define DIR_2_PIN 22 #define DIR_2_PLUS REG_WRITE(GPIO_OUT_W1TS_REG, BIT22) #define DIR_2_MINUS REG_WRITE(GPIO_OUT_W1TC_REG, BIT22) /*==================================================================*/ #define OUT_00_PIN 2 #define OUT_00_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT2) #define OUT_00_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT2) #define OUT_01_PIN 4 #define OUT_01_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT4) #define OUT_01_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT4) #define OUT_02_PIN 25 #define OUT_02_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT25) #define OUT_02_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT25) /*==================================================================*/ #define IN_00_PIN 26 #define IN_00 REG_READ(GPIO_IN_REG) & BIT26 //26 #define IN_01_PIN 27 #define IN_01 REG_READ(GPIO_IN_REG) & BIT27 //27 #define IN_02_PIN 32 #define IN_02 REG_READ(GPIO_IN1_REG) & BIT0 //32 -32 #define IN_03_PIN 33 #define IN_03 REG_READ(GPIO_IN1_REG) & BIT1 //33 -32 #define IN_04_PIN 34 #define IN_04 REG_READ(GPIO_IN1_REG) & BIT2 //34 -32 #define IN_05_PIN 35 #define IN_05 REG_READ(GPIO_IN1_REG) & BIT3 //35 -32 #define IN_06_PIN 36 #define IN_06 REG_READ(GPIO_IN1_REG) & BIT4 //36 -32 #define IN_07_PIN 39 #define IN_07 REG_READ(GPIO_IN1_REG) & BIT7 //39 -32 /*==================================================================*/ #define CTRL_DIRSETUP 0b00000001 #define CTRL_MAXVEL 0b00000010 #define CTRL_MAXACC 0b00000100 #define CTRL_READY 0b01000000 #define CTRL_ENABLE 0b10000000 #define IO_00 0b00000001 #define IO_01 0b00000010 #define IO_02 0b00000100 #define IO_03 0b00001000 #define IO_04 0b00010000 #define IO_05 0b00100000 #define IO_06 0b01000000 #define IO_07 0b10000000 /*==================================================================*/ char packetBuffer[50]; // buffer for receiving and sending data EthernetUDP Udp; // An EthernetUDP instance to let us send and receive packets over UDP struct cmdPacket { uint8_t control; uint8_t io; int32_t pos[3]; float vel[3]; } cmd = { 0, 0, 0, 0, 0, 0.0f, 0.0f, 0.0f }; struct fbPacket { uint8_t control; uint8_t io; int32_t pos[3]; float vel[3]; } fb = { 0, 0, 0, 0, 0, 0.0f, 0.0f, 0.0f }; /*==================================================================*/ volatile unsigned long ul_dirSetup[3] = { 1000, 1000, 1000 }; // x 25 nanosec volatile unsigned long ul_min_T[3] = { 10000, 10000, 10000 }; // x 25 nanosec volatile unsigned long ul_maxAcc_x2[3] = { 1, 1, 1 }; // max acceleration step/sec2 /*==================================================================*/ volatile long l_predictive_pos[3]; volatile long l_cmd_pos_last[3]; volatile long l_cmd_pos[3] = { 0, 0, 0 }; volatile unsigned long ul_cmd_T[3]; volatile unsigned long ul_accelStep[3] = { 0, 0, 0 }; volatile unsigned long ul_T[3] = { 0, 0, 0 }; volatile unsigned long ul_TH[3] = { 0, 0, 0 }; volatile unsigned long ul_TL[3] = { 0, 0, 0 }; volatile bool b_math[3] = { false, false, false }; volatile bool b_dirSignal[3] = { LOW, LOW, LOW }; volatile bool b_dirChange[3] = { false, false, false }; unsigned long ul_watchdog; hw_timer_t* stepGen_0 = NULL; hw_timer_t* stepGen_1 = NULL; hw_timer_t* stepGen_2 = NULL; /*==================================================================*/ void IRAM_ATTR onTime_0() { volatile static bool b_stepState = LOW; volatile static bool b_dirState = LOW; if (b_stepState == LOW) { // end of period if (ul_T[0]) { // van periódisidő tehát impulzust kell indítani if (!b_dirChange[0]) { //jó az irány, új impulzus indítása step L-H átmenet STEP_0_H; timerAlarmWrite(stepGen_0, ul_TH[0], true); b_stepState = HIGH; b_dirState ? fb.pos[0]++ : fb.pos[0]--; b_math[0] = true; // kiszámítjuk a következő értékeket } else { // irányt kell váltani if (b_dirSignal[0]) { DIR_0_PLUS; b_dirState = HIGH; } else { DIR_0_MINUS; b_dirState = LOW; } timerAlarmWrite(stepGen_0, ul_dirSetup[0], true); b_dirChange[0] = false; } } else { // nem kell impulzus timerAlarmWrite(stepGen_0, 4000ul, true); // 0,1 millis scan b_math[0] = true; // kiszámítjuk a következő értékeket } } else { //priódus közepe step H-L átmenet STEP_0_L; timerAlarmWrite(stepGen_0, ul_TL[0], true); b_stepState = LOW; } } /*==================================================================*/ void IRAM_ATTR onTime_1() { volatile static bool b_stepState = LOW; volatile static bool b_dirState = LOW; if (b_stepState == LOW) { // periódus vége if (ul_T[1]) { // van periódisidő tehát impulzust kell indítani if (!b_dirChange[1]) { //jó az irány, új impulzus indítása step L-H átmenet STEP_1_H; timerAlarmWrite(stepGen_1, ul_TH[1], true); b_stepState = HIGH; b_dirState ? fb.pos[1]++ : fb.pos[1]--; b_math[1] = true; // kiszámítjuk a következő értékeket } else { // irányt kell váltani if (b_dirSignal[1]) { DIR_1_PLUS; b_dirState = HIGH; } else { DIR_1_MINUS; b_dirState = LOW; } timerAlarmWrite(stepGen_1, ul_dirSetup[1], true); b_dirChange[1] = false; } } else { // nem kell impulzus timerAlarmWrite(stepGen_1, 4000ul, true); // 0,1 millis scan b_math[1] = true; // kiszámítjuk a következő értékeket } } else { //priódus közepe step H-L átmenet STEP_1_L; timerAlarmWrite(stepGen_1, ul_TL[1], true); b_stepState = LOW; } } /*==================================================================*/ void IRAM_ATTR onTime_2() { volatile static bool b_stepState = LOW; volatile static bool b_dirState = LOW; if (b_stepState == LOW) { // periódus vége if (ul_T[2]) { // van periódisidő tehát impulzust kell indítani if (!b_dirChange[2]) { //jó az irány, új impulzus indítása step L-H átmenet STEP_2_H; timerAlarmWrite(stepGen_2, ul_TH[2], true); b_stepState = HIGH; b_dirState ? fb.pos[2]++ : fb.pos[2]--; b_math[2] = true; // kiszámítjuk a következő értékeket } else { // irányt kell váltani if (b_dirSignal[2]) { DIR_2_PLUS; b_dirState = HIGH; } else { DIR_2_MINUS; b_dirState = LOW; } timerAlarmWrite(stepGen_2, ul_dirSetup[2], true); b_dirChange[2] = false; } } else { // nem kell impulzus timerAlarmWrite(stepGen_2, 4000ul, true); // 0,1 millis scan b_math[2] = true; // kiszámítjuk a következő értékeket } } else { //priódus közepe step H-L átmenet STEP_2_L; timerAlarmWrite(stepGen_2, ul_TL[2], true); b_stepState = LOW; } } /*==================================================================*/ void IRAM_ATTR newT(int i) //T kiszámítása { ul_T[i] = 40000000.0f / sqrtf((float)(ul_accelStep[i] * ul_maxAcc_x2[i])); ul_TH[i] = ul_T[i] >> 1; ul_TL[i] = ul_T[i] - ul_TH[i]; } /*==================================================================*/ void IRAM_ATTR deceleration(int i) { if (ul_accelStep[i]) { ul_accelStep[i]--; if (ul_accelStep[i]) newT(i); else ul_T[i] = 0; } } /*==================================================================*/ void IRAM_ATTR acceleration(int i) { if (cmd.control & CTRL_ENABLE) { if (ul_T[i] > ul_min_T[i] || !ul_T[i]) { ul_accelStep[i]++; newT(i); } } else deceleration(i); } /*==================================================================*/ void IRAM_ATTR outputHandler() { if (cmd.control & CTRL_ENABLE) { (cmd.io & IO_00) ? OUT_00_H : OUT_00_L; (cmd.io & IO_01) ? OUT_01_H : OUT_01_L; (cmd.io & IO_02) ? OUT_02_H : OUT_02_L; } else { OUT_00_L; OUT_01_L; OUT_02_L; } } /*==================================================================*/ void IRAM_ATTR inputHandler() { (IN_00) ? fb.io = IO_00 : fb.io = 0; if (IN_01) fb.io |= IO_01; if (IN_02) fb.io |= IO_02; if (IN_03) fb.io |= IO_03; } /*==================================================================*/ void IRAM_ATTR commandHandler() { if (cmd.control & CTRL_READY) { for (int i = 0; i < 3; i++) { if (cmd.vel[i] > 0.0f) ul_cmd_T[i] = (unsigned long)(40000000.0f / cmd.vel[i]); else if (cmd.vel[i] < 0.0f) ul_cmd_T[i] = (unsigned long)(40000000.0f / -cmd.vel[i]); else ul_cmd_T[i] = 40000000ul; l_cmd_pos_last[i] = l_cmd_pos[i]; l_cmd_pos[i] = cmd.pos[i]; l_predictive_pos[i] = l_cmd_pos[i] + l_cmd_pos[i] - l_cmd_pos_last[i]; } } if (!(fb.control & CTRL_READY)) { if ((fb.control & CTRL_DIRSETUP) && (fb.control & CTRL_MAXACC) && (fb.control & CTRL_MAXVEL)) { fb.control |= CTRL_READY; //log_printf("READY\n"); } else if (cmd.control & CTRL_DIRSETUP) { fb.control |= CTRL_DIRSETUP; for (int i = 0; i < 3; i++) ul_dirSetup[i] = cmd.pos[i] / 25; // 25ns / timer tic } else if (cmd.control & CTRL_MAXVEL) { fb.control |= CTRL_MAXVEL; for (int i = 0; i < 3; i++) ul_min_T[i] = 40000000ul / cmd.pos[i]; } else if (cmd.control & CTRL_MAXACC) { fb.control |= CTRL_MAXACC; for (int i = 0; i < 3; i++) ul_maxAcc_x2[i] = cmd.pos[i] << 1; } } } /*==================================================================*/ void IRAM_ATTR loop_Core0(void* parameter) { for (;;) { if (Udp.parsePacket()) { // >= sizeof(cmd) + 1 Udp.read(packetBuffer, sizeof(cmd) + 1); uint8_t chk = 71; for (int i = 0; i < sizeof(cmd); i++) chk ^= packetBuffer[i]; if (packetBuffer[sizeof(cmd)] == chk) { memcpy(&cmd, &packetBuffer, sizeof(cmd)); commandHandler(); ul_watchdog = millis(); } inputHandler(); for (int i = 0; i < 3; i++) { unsigned long ul_t = ul_T[i]; if (ul_t) b_dirSignal[i] ? fb.vel[i] = 40000000.0f / (float)ul_t : fb.vel[i] = -40000000.0f / (float)ul_t; else fb.vel[i] = 0.0f; } memcpy(&packetBuffer, &fb, sizeof(fb)); Udp.beginPacket(Udp.remoteIP(), Udp.remotePort()); Udp.write(packetBuffer, sizeof(fb)); Udp.endPacket(); outputHandler(); } if (millis() - ul_watchdog > 10ul) { fb.control = 0; cmd.control = 0; outputHandler(); ul_watchdog = millis(); } } } /*==================================================================*/ void setup_Core0(void* parameter) { pinMode(OUT_00_PIN, OUTPUT); digitalWrite(OUT_00_PIN, 0); pinMode(OUT_01_PIN, OUTPUT); digitalWrite(OUT_01_PIN, 0); pinMode(OUT_02_PIN, OUTPUT); digitalWrite(OUT_02_PIN, 0); pinMode(IN_00_PIN, INPUT_PULLUP); pinMode(IN_01_PIN, INPUT_PULLUP); pinMode(IN_02_PIN, INPUT_PULLUP); pinMode(IN_03_PIN, INPUT_PULLUP); //pinMode(IN_04_PIN, INPUT); //pinMode(IN_05_PIN, INPUT); //pinMode(IN_06_PIN, INPUT); //pinMode(IN_07_PIN, INPUT); delay(100); Ethernet.init(SPI_CS_PIN); /* You can use Ethernet.init(pin) to configure the CS pin */ Ethernet.begin(mac, ip); /* start the Ethernet */ delay(100); Udp.begin(port); /* start UDP */ delay(100); xTaskCreatePinnedToCore( loop_Core0, // Task function. "loopTask_Core0", // name of task. 3000, // Stack size of task NULL, // parameter of the task 0, // priority of the task NULL, // Task handle to keep track of created task 0); // pin task to core 0 disableCore0WDT(); vTaskDelete(NULL); } /*==================================================================*/ void setup() { pinMode(STEP_0_PIN, OUTPUT); STEP_0_L; pinMode(DIR_0_PIN, OUTPUT); DIR_0_MINUS; pinMode(STEP_1_PIN, OUTPUT); STEP_1_L; pinMode(DIR_1_PIN, OUTPUT); DIR_1_MINUS; pinMode(STEP_2_PIN, OUTPUT); STEP_2_L; pinMode(DIR_2_PIN, OUTPUT); DIR_2_MINUS; // Configure the Prescaler // 80 000 000 / 2 = 40 000 000 tics/second ===> 25 ns/tic stepGen_0 = timerBegin(0, 2, true); stepGen_1 = timerBegin(1, 2, true); stepGen_2 = timerBegin(2, 2, true); timerAttachInterrupt(stepGen_0, &onTime_0, true); timerAttachInterrupt(stepGen_1, &onTime_1, true); timerAttachInterrupt(stepGen_2, &onTime_2, true); timerAlarmWrite(stepGen_0, 40000000, true); timerAlarmWrite(stepGen_1, 40000000, true); timerAlarmWrite(stepGen_2, 40000000, true); timerAlarmEnable(stepGen_0); timerAlarmEnable(stepGen_1); timerAlarmEnable(stepGen_2); xTaskCreatePinnedToCore( setup_Core0, // Task function. "setup_Core0Task", // name of task. 1000, // Stack size of task NULL, // parameter of the task 0, // priority of the task NULL, // Task handle to keep track of created task 0); // pin task to core 0 } /*==================================================================*/ void IRAM_ATTR loop() { for (int i = 0; i < 3; i++) { if (b_math[i]) { b_math[i] = false; if (ul_accelStep[i] == 0) { // áll a motor long l_pos_error = l_predictive_pos[i] - fb.pos[i]; if (l_pos_error != 0) { // ha van hiba if ((l_pos_error > 0 && b_dirSignal[i] == HIGH) || (l_pos_error < 0 && b_dirSignal[i] == LOW)) // jó az irány acceleration(i); else { // irányt kell váltani (l_pos_error > 0) ? b_dirSignal[i] = HIGH : b_dirSignal[i] = LOW; b_dirChange[i] = true; } } } else { // mozgásban van a motor if ((cmd.vel[i] > 0.0f && b_dirSignal[i] == HIGH) || (cmd.vel[i] < 0.0f && b_dirSignal[i] == LOW)) { // jó az irány if (ul_T[i] > ul_cmd_T[i]) // kicsi a sebesség acceleration(i); else if (ul_T[i] < ul_cmd_T[i]) // nagy a sebesség deceleration(i); } else // rossz az irány vagy 0 a célsebesség deceleration(i); } } } }