/* This is a hardware step generator with dual core ESP32 and W5500 module
* for LinuxCNC
*
* Copyright 2021 Juhász Zoltán <juhasz.zoltan at freemail dot hu>
*
* 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 <arduino.h>
#include <Ethernet.h>
#include <EthernetUdp.h>
/*==================================================================*/
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);
}
}
}
}