HAL2UDP (1) kod z komentarzem

masz pytania?
Poniżej znajduje się przetłumaczony i obszernie skomentowany kod main.cpp projektu HAL2UDP. Komentarze wyjaśniają zarówno poszczególne sekcje kodu, jak i dotykają ogólnych koncepcji używanych w tym projekcie.
https://github.com/jzolee/HAL2UDP/blob/main/src/main.cpp
      
/*
 * Zewnętrzny generator kroków i interfejs IO dla LinuxCNC poprzez Ethernet
 * z użyciem dwurdzeniowych modułów ESP32 i W5500.
 *
 * Copyright 2022 Juhász Zoltán 
 * 
 * wersja: 20220330
 *
 * Ten program jest wolnym oprogramowaniem; możesz go rozpowszechniać i/lub modyfikować
 * na warunkach GNU General Public License opublikowanej przez Fundację Wolnego Oprogramowania; 
 * albo w wersji 2 Licencji, albo (według twojego wyboru) w dowolnej późniejszej wersji.
 *
 * Ten program jest dystrybuowany w nadziei, że będzie użyteczny,
 * ale BEZ JAKIEJKOLWIEK GWARANCJI; bez nawet domyślnej gwarancji
 * przydatności handlowej lub przydatności do określonego celu. Zobacz GNU General Public License
 * po więcej szczegółów.
 *
 * Powinieneś otrzymać kopię GNU General Public License
 * razem z tym programem; jeśli nie, napisz do Fundacji Wolnego Oprogramowania, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

/*
 *  Sprzęt:
 *
 *           3v3 -- 3.3v W5500 (zasilanie)
 *           GND -- GND  W5500 (uziemienie)
 *           GND -- (DB15- 7)
 *           GND -- (DB15- 8)
 *
 *       GPIO  1 -> OUT-03 lub PWM-03
 *       GPIO  2 -> OUT-00 lub PWM-00  -[100R]-(DB15 - 9) & onboard blue LED
 *       GPIO  4 -> OUT-01 lub PWM-01  -[100R]-(DB15 -10)
 *       GPIO  5 -> W5500 SCS (Chip Select)
 *       GPIO 12 -> step-0            -[100R]-(DB15 - 1)
 *       GPIO 13 -> dir-0             -[100R]-(DB15 - 2)
 *       GPIO 14 -> OUT-04 lub PWM-04
 *       GPIO 15 -> OUT-05 lub PWM-05
 * (RX2) GPIO 16 -> step-1            -[100R]-(DB15 - 3)
 * (TX2) GPIO 17 -> dir-1             -[100R]-(DB15 - 4)
 *       GPIO 18 -> W5500 SCLK (Clock)
 *       GPIO 19 
 *       GPIO 21 -> step-2            -[100R]-(DB15 - 5)
 *       GPIO 22 -> dir-2             -[100R]-(DB15 - 6)
 *       GPIO 23 -> W5500 MOSI (Master Out Slave In)
 *       GPIO 25 -> OUT-02 lub PWM-02  -[100R]-(DB15 -11)
 *       GPIO 26 
 *       GPIO 27 
 *       GPIO 32 
 *       GPIO 33 
 *       GPIO 34 
 *       GPIO 35 
 *  (VP) GPIO 36 
 *  (VN) GPIO 39 
 */

/*
 *  NOTATKI GPIO WROOM 32
 *
 *  https://randomnerdtutorials.com/esp32-pinout-reference-gpios/
 *
 *  GPIO    Wejście           Wyjście      Notatki
 *
 *  0     ? podciągnięty ?   ? OK ?        generuje sygnał PWM przy uruchamianiu
 *  1    !! pin TX !!        ? OK ?        dane debugowe przy uruchamianiu
 *  2       OK              OK              połączony z diodą LED na płytce
 *  3     ? OK ?            !! pin RX !!    stan wysoki przy uruchamianiu
 *  4       OK              OK
 *  5       OK              OK              generuje sygnał PWM przy uruchamianiu
 *  6-11    X               X               połączone z wbudowaną pamięcią SPI flash
 *  12    ? OK ?            OK              błąd uruchomienia, jeśli wysokie
 *  13      OK              OK
 *  14      OK              OK              generuje sygnał PWM przy uruchamianiu
 *  15      OK              OK              generuje sygnał PWM przy uruchamianiu
 *  16      OK              OK
 *  17      OK              OK
 *  18      OK              OK
 *  19      OK              OK
 *  21      OK              OK
 *  22      OK              OK
 *  23      OK              OK
 *  25      OK              OK
 *  26      OK              OK
 *  27      OK              OK
 *  32      OK              OK
 *  33      OK              OK
 *  34      OK              --          tylko wejście, brak podciągnięcia
 *  35      OK              --          tylko wejście, brak podciągnięcia
 *  36      OK              --          tylko wejście, brak podciągnięcia
 *  39      OK              --          tylko wejście, brak podciągnięcia
 */

#include   // Biblioteka Arduino, zapewniająca podstawowe funkcje

#include  // Biblioteka do obsługi Ethernetu
#include  // Biblioteka do obsługi komunikacji UDP przez Ethernet

/*==================================================================*/

// Adres MAC dla Ethernetu (zwykle unikalny składający się z 6 bajtów)
byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };

// Statyczny adres IP, który będzie używany przez naszą aplikację
IPAddress ip(192, 168, 96, 54); // Adres IP naszego ESP32

// Port, na którym nasz serwer UDP będzie nasłuchiwał
unsigned int port = 58427;

/*==================================================================*/

// Zdefiniowanie pinu CS (Chip Select) dla Ethernetu
#define SPI_CS_PIN 5 // Zastępując wartość w Ethernet_mod/src/utility/w5100.h

/*==================================================================*/

// Definicje pinów dla kroków i kierunków dla trzech osi
#define STEP_0_PIN 12
#define STEP_0_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT12) // Ustawienie pinu STEP 0 w stan wysoki
#define STEP_0_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT12) // Ustawienie pinu STEP 0 w stan niski
#define DIR_0_PIN 13
#define DIR_0_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT13) // Ustawienie pinu DIR 0 w stan wysoki
#define DIR_0_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT13) // Ustawienie pinu DIR 0 w stan niski

#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_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT17)
#define DIR_1_L 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_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT22)
#define DIR_2_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT22)

/*==================================================================*/

// Definicje pinów dla wyjść (OUT) oraz PWM
#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 OUT_03_PIN 1
#define OUT_03_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT1)
#define OUT_03_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT1)
#define OUT_04_PIN 14
#define OUT_04_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT14)
#define OUT_04_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT14)
#define OUT_05_PIN 15
#define OUT_05_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT15)
#define OUT_05_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT15)

/*==================================================================*/

// Definicje pinów wejściowych (IN)
#define IN_00_PIN 26
#define IN_00 REG_READ(GPIO_IN_REG) & BIT26 // Zmienne do odczytu stanów wejść
#define IN_01_PIN 27
#define IN_01 REG_READ(GPIO_IN_REG) & BIT27
#define IN_02_PIN 32
#define IN_02 REG_READ(GPIO_IN1_REG) & BIT0
#define IN_03_PIN 33
#define IN_03 REG_READ(GPIO_IN1_REG) & BIT1
#define IN_04_PIN 34
#define IN_04 REG_READ(GPIO_IN1_REG) & BIT2
#define IN_05_PIN 35
#define IN_05 REG_READ(GPIO_IN1_REG) & BIT3
#define IN_06_PIN 36
#define IN_06 REG_READ(GPIO_IN1_REG) & BIT4
#define IN_07_PIN 39
#define IN_07 REG_READ(GPIO_IN1_REG) & BIT7

/*==================================================================*/

// Definicje kontrolne bitów do zarządzania różnymi opcjami
#define CTRL_DIRSETUP 0b00000001 // Bit do ustawiania kierunku
#define CTRL_ACCEL    0b00000010 // Bit do aktywacji przyspieszenia
#define CTRL_PWMFREQ  0b00000100 // Bit do definiowania częstotliwości PWM
#define CTRL_READY    0b01000000 // Bit wskazujący, że system jest gotowy
#define CTRL_ENABLE   0b10000000 // Bit włączający/wyłączający funkcję

// Definicje wejść/wyjść IO
#define IO_00 0b00000001 // Definicja dla IO 0
#define IO_01 0b00000010 // Definicja dla IO 1
#define IO_02 0b00000100 // Definicja dla IO 2
#define IO_03 0b00001000 // Definicja dla IO 3
#define IO_04 0b00010000 // Definicja dla IO 4
#define IO_05 0b00100000 // Definicja dla IO 5
#define IO_06 0b01000000 // Definicja dla IO 6
#define IO_07 0b10000000 // Definicja dla IO 7

/*==================================================================*/

// Tworzenie instancji klasy EthernetUDP do zarządzania komunikacją UDP
EthernetUDP Udp; // Instancja do przesyłania i odbierania pakietów UDP

// Struktura dla pakietu komend
struct cmdPacket {
    uint8_t control; // Kontrola komendy
    uint8_t io; // Wejście/wyjście
    uint16_t pwm[6]; // Tablica wartości PWM (do 6 wejść)
    int32_t pos[3]; // Pozycje dla 3 osi
    float vel[3]; // Prędkości dla 3 osi
} cmd = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0.0f, 0.0f }; // Inicjalizacja do wartości domyślnych

// Struktura dla pakietu informacji zwrotnej
struct fbPacket {
    uint8_t control; // Kontrola informacji zwrotnej
    uint8_t io; // Stan wejść/wyjść
    int32_t pos[3]; // Pozycje osi
    float vel[3]; // Prędkości osi
} fb = { 0, 0, 0, 0, 0, 0.0f, 0.0f, 0.0f }; // Inicjalizacja wartości domyślnych

/*==================================================================*/

// Zmienne volatile do przechowywania ustawień i stanu
volatile unsigned long ul_dirSetup[3] = { 1000, 1000, 1000 }; // Ustawienia kierunku (czas w ns)
volatile float f_accel_x2[3] = { 1000.0, 1000.0, 1000.0 }; // Przyspieszenie * 2 w krokach na sekundę kwadrat

/*==================================================================*/

// Zmienne drobne do cech osi
volatile unsigned long ul_cmd_T[3]; // Zmienne do przechowywania czasów komend
unsigned long ul_accelStep[3] = { 0, 0, 0 }; // Zmienne do przechowywania liczby kroków w przyspieszeniu

// Zmienne do przechowywania różnych ważnych czasów
volatile unsigned long ul_T[3] = { 0, 0, 0 }; // Czas cyklu
volatile unsigned long ul_TH[3] = { 0, 0, 0 }; // Czas wyższy
volatile unsigned long ul_TL[3] = { 0, 0, 0 }; // Czas niższy
volatile bool b_math[3] = { false, false, false }; // Flagi do obliczeń pozycjonowania
volatile bool b_dirSignal[3] = { LOW, LOW, LOW }; // Flagi dla kierunków
volatile bool b_dirChange[3] = { false, false, false }; // Flagi mówiące, że musimy zmienić kierunek

const uint8_t pwm_pin[6] = { OUT_00_PIN, OUT_01_PIN, OUT_02_PIN, OUT_03_PIN, OUT_04_PIN, OUT_05_PIN }; // Piny PWM
bool pwm_enable[6] = { false, false, false, false, false, false }; // Flagi włączające PWM dla każdego kanału

/*==================================================================*/

// Timery do generacji kroków dla trzech osi
hw_timer_t* stepGen_0 = NULL; // Timer dla osi 0
hw_timer_t* stepGen_1 = NULL; // Timer dla osi 1
hw_timer_t* stepGen_2 = NULL; // Timer dla osi 2

/*==================================================================*/

// Funkcja do szybkiego obliczania odwrotności pierwiastka
float IRAM_ATTR fastInvSqrt(const float x) {
    const float xhalf = x * 0.5f; // Połowa wartości x
    union {
        float x; // Typ float
        uint32_t i; // Typ 32 bitowy
    } u = { .x = x }; // Inicjalizacja unii wartością x
    u.i = 0x5f3759df - (u.i >> 1); // Algorytm szybkiej odwrotności pierwiastka
    return u.x * (1.5f - xhalf * u.x * u.x); // Zwracanie wyniku
}

/*==================================================================*/

// Obsługuje timer dla osi 0
void IRAM_ATTR onTime_0() {
    static bool b_stepState = LOW; // Stan kroku (niski na początku)
    static bool b_dirState = LOW; // Stan kierunku

    if (b_stepState == LOW) { // Koniec cyklu, wykonywanie impulsu
        if (ul_T[0]) { // Istnieje czas, więc należy zacząć impuls
            if (!b_dirChange[0]) { // Jeśli kierunek jest poprawny
                STEP_0_H; // Ustaw HIGH na pinie STEP 0
                timerAlarmWrite(stepGen_0, ul_TH[0], true); // Ustaw alarm timera
                b_stepState = HIGH; // Zmień stan na wysoki
                b_dirState ? fb.pos[0]++ : fb.pos[0]--; // Zmiana pozycji w zależności od kierunku
                b_math[0] = true; // Ustaw flagę do obliczeń
            } else { // Musimy zmienić kierunek
                if (b_dirSignal[0]) {
                    DIR_0_H; // Ustaw HIGH na pinie kierunku
                    b_dirState = HIGH;
                } else {
                    DIR_0_L; // Ustaw LOW na pinie kierunku
                    b_dirState = LOW;
                }
                timerAlarmWrite(stepGen_0, ul_dirSetup[0], true); // Ustaw nowy czas ustalony dla kierunku
                b_dirChange[0] = false; // Resetuj flagę zmiany kierunku
            }
        } else { // Nie wymaga impulsu
            timerAlarmWrite(stepGen_0, 4000ul, true); // Ustaw alarm timera na mały czas
            b_math[0] = true; // Flaga dla obliczeń
        }
    } else { // Środek cyklu
        STEP_0_L; // Ustaw LOW na pinie STEP 0
        timerAlarmWrite(stepGen_0, ul_TL[0], true); // Ustaw alarm na czas niższy
        b_stepState = LOW; // Zmień stan na niski
    }
}
/*==================================================================*/

// Obsługuje timer dla osi 1
void IRAM_ATTR onTime_1() {
    static bool b_stepState = LOW; // Stan kroku
    static bool b_dirState = LOW; // Stan kierunku

    if (b_stepState == LOW) { // Koniec cyklu
        if (ul_T[1]) { // Istnieje czas
            if (!b_dirChange[1]) { // Kierunek poprawny
                STEP_1_H; // Impuls HIGH
                timerAlarmWrite(stepGen_1, ul_TH[1], true);
                b_stepState = HIGH; // Zmień stan
                b_dirState ? fb.pos[1]++ : fb.pos[1]--; // Zmiana pozycji
                b_math[1] = true; // Ustaw flagę
            } else { // Zmiana kierunku
                if (b_dirSignal[1]) {
                    DIR_1_H;
                    b_dirState = HIGH;
                } else {
                    DIR_1_L;
                    b_dirState = LOW;
                }
                timerAlarmWrite(stepGen_1, ul_dirSetup[1], true);
                b_dirChange[1] = false; // Reset flagi
            }
        } else { // Nie wymaga impulsu
            timerAlarmWrite(stepGen_1, 4000ul, true);
            b_math[1] = true;
        }
    } else { // Środek cyklu
        STEP_1_L; // Ustaw LOW
        timerAlarmWrite(stepGen_1, ul_TL[1], true);
        b_stepState = LOW; 
    }
}

/*==================================================================*/

// Obsługuje timer dla osi 2
void IRAM_ATTR onTime_2() {
    static bool b_stepState = LOW; 
    static bool b_dirState = LOW; 

    if (b_stepState == LOW) { 
        if (ul_T[2]) { 
            if (!b_dirChange[2]) { 
                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; 
            } else { 
                if (b_dirSignal[2]) {
                    DIR_2_H; 
                    b_dirState = HIGH;
                } else {
                    DIR_2_L; 
                    b_dirState = LOW;
                }
                timerAlarmWrite(stepGen_2, ul_dirSetup[2], true);
                b_dirChange[2] = false; 
            }
        } else { 
            timerAlarmWrite(stepGen_2, 4000ul, true); 
            b_math[2] = true; 
        }
    } else { 
        STEP_2_L; 
        timerAlarmWrite(stepGen_2, ul_TL[2], true);
        b_stepState = LOW; 
    }
}

/*==================================================================*/

// Oblicza czas periodyczny w oparciu o przyspieszenie
void IRAM_ATTR newT(const int i) {
    // Użycie algorytmu szybkiego odwrotności pierwiastka dla wydajności
    ul_T[i] = 40000000.0f * fastInvSqrt((float)ul_accelStep[i] * f_accel_x2[i]); // Sqrt wzór
    ul_TH[i] = ul_T[i] >> 1; // Ustalanie połowy
    ul_TL[i] = ul_T[i] - ul_TH[i]; // Czas niższy
}

/*==================================================================*/

// Dekceleracja na podstawie liczby kroków
void IRAM_ATTR deceleration(const int i) {
    if (ul_accelStep[i]) {
        ul_accelStep[i]--; // Decrement steps
        if (ul_accelStep[i])
            newT(i); // Nowe przeliczenie czasu
        else
            ul_T[i] = 0; // Ustawienie zerowego czasu
    }
}

/*==================================================================*/

// Przyspiesza w zależności od kontroli aktywnej
void IRAM_ATTR acceleration(const int i) {
    if (cmd.control & CTRL_ENABLE) {
        ul_accelStep[i]++; // Inkrementacja
        newT(i); // Obliczenie nowego czasu
    } else
        deceleration(i); // Dekceleracja w przeciwnym razie
}

/*==================================================================*/

// Obsługuje wyjścia (wyjście dla PWM i innych sygnałów)
void IRAM_ATTR outputHandler() {
    static int last_pwm[6] = { 0, 0, 0, 0, 0, 0 }; // Ostatnie wartości PWM dla każdego kanału

    bool enable = cmd.control & CTRL_ENABLE; // Sprawdzamy, czy kontrola jest włączona

    // Obsługa wyjścia PWM dla pierwszego pinu
    if (pwm_enable[0]) { 
        if (enable) { 
            if (last_pwm[0] != cmd.pwm[0]) { // Sprawdza zmianę PWM
                last_pwm[0] = cmd.pwm[0]; 
                ledcWrite(0, last_pwm[0]); // Zapisuje nową wartość PWM
            }
        } else {
            ledcWrite(0, 0); // Ustawienie PWM na 0
            last_pwm[0] = 0; 
        }
    } else {
        if (enable)
            (cmd.io & IO_00) ? OUT_00_H : OUT_00_L; // Ustawia stan wyjścia na podstawie aktualnego IO
        else
            OUT_00_L; // Gdy wyłączone, ustaw na niski
    }

    // Powtarzamy analogicznie dla kolejnych kanałów PWM
    if (pwm_enable[1]) { 
        if (enable) { 
            if (last_pwm[1] != cmd.pwm[1]) { 
                last_pwm[1] = cmd.pwm[1]; 
                ledcWrite(2, last_pwm[1]); 
            }
        } else {
            ledcWrite(2, 0); 
            last_pwm[1] = 0; 
        }
    } else {
        if (enable)
            (cmd.io & IO_01) ? OUT_01_H : OUT_01_L; 
        else
            OUT_01_L; 
    }

    if (pwm_enable[2]) { 
        if (enable) { 
            if (last_pwm[2] != cmd.pwm[2]) { 
                last_pwm[2] = cmd.pwm[2]; 
                ledcWrite(4, last_pwm[2]); 
            }
        } else {
            ledcWrite(4, 0); 
            last_pwm[2] = 0; 
        }
    } else {
        if (enable)
            (cmd.io & IO_02) ? OUT_02_H : OUT_02_L; 
        else
            OUT_02_L; 
    }

    if (pwm_enable[3]) { 
        if (enable) { 
            if (last_pwm[3] != cmd.pwm[3]) { 
                last_pwm[3] = cmd.pwm[3]; 
                ledcWrite(6, last_pwm[3]); 
            }
        } else {
            ledcWrite(6, 0); 
            last_pwm[3] = 0; 
        }
    } else {
        if (enable)
            (cmd.io & IO_03) ? OUT_03_H : OUT_03_L; 
        else
            OUT_03_L; 
    }

    if (pwm_enable[4]) { 
        if (enable) { 
            if (last_pwm[4] != cmd.pwm[4]) { 
                last_pwm[4] = cmd.pwm[4]; 
                ledcWrite(8, last_pwm[4]); 
            }
        } else {
            ledcWrite(8, 0); 
            last_pwm[4] = 0; 
        }
    } else {
        if (enable)
            (cmd.io & IO_04) ? OUT_04_H : OUT_04_L; 
        else
            OUT_04_L; 
    }

    if (pwm_enable[5]) { 
        if (enable) { 
            if (last_pwm[5] != cmd.pwm[5]) { 
                last_pwm[5] = cmd.pwm[5]; 
                ledcWrite(10, last_pwm[5]); 
            }
        } else {
            ledcWrite(10, 0); 
            last_pwm[5] = 0; 
        }
    } else {
        if (enable)
            (cmd.io & IO_05) ? OUT_05_H : OUT_05_L; 
        else
            OUT_05_L; 
    }
}

/*==================================================================*/

// Obsługuje sygnały wejściowe
void IRAM_ATTR inputHandler() {
    // Odczytuje stany wejść i ustawia flagi
    (IN_00) ? fb.io = IO_00 : fb.io = 0; // Ustawienie flags io
    if (IN_01)
        fb.io |= IO_01;
    if (IN_02)
        fb.io |= IO_02;
    if (IN_03)
        fb.io |= IO_03;
    if (IN_04)
        fb.io |= IO_04;
    if (IN_05)
        fb.io |= IO_05;
    if (IN_06)
        fb.io |= IO_06;
    if (IN_07)
        fb.io |= IO_07;
}

/*==================================================================*/

// Obsługuje komendy przychodzące
void IRAM_ATTR commandHandler() {
    if (cmd.control & CTRL_READY) { // Sprawdzenie, czy kontrola jest gotowa
        for (int i = 0; i < 3; i++) {
            // Ustalanie czasu na podstawie podanej prędkości
            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; // Domyślna wartość
        }
    }

    // Weryfikacja czy już jesteśmy gotowi na kolejne komendy
    if (!(fb.control & CTRL_READY)) {
        if ((fb.control & CTRL_DIRSETUP)
            && (fb.control & CTRL_ACCEL)
            && (fb.control & CTRL_PWMFREQ)) {
            fb.control |= CTRL_READY; // Ustawienie gotowości
            //log_printf("READYn"); // Logowanie gotowości (jeśli jest funkcja logowania)
        } else if (cmd.control & CTRL_DIRSETUP) { // Ustawienie kierunku
            fb.control |= CTRL_DIRSETUP; // Ustawienie flagi
            for (int i = 0; i < 3; i++)
                ul_dirSetup[i] = cmd.pos[i] / 25; // Ustalanie ustawień kierunku
        } else if (cmd.control & CTRL_ACCEL) { // Ustawienie przyspieszenia
            fb.control |= CTRL_ACCEL; // Ustawienie flagi
            for (int i = 0; i < 3; i++)
                f_accel_x2[i] = (float)cmd.pos[i] * 2.0; // Podwajanie wartości przyspieszenia
        } else if (cmd.control & CTRL_PWMFREQ) { // Ustalanie częstotliwości PWM
            fb.control |= CTRL_PWMFREQ; // Ustawienie flagi
            for (int i = 0; i < 6; i++) {
                if (cmd.pwm[i]) { // Jeśli wartość PWM jest podana
                    ledcAttachPin(pwm_pin[i], i * 2); // Dołączenie pinu PWM
                    ledcSetup(i * 2, cmd.pwm[i], 10); // Ustawienia PWM
                    ledcWrite(i * 2, 0); // Inicjalizacja wyjścia
                    pwm_enable[i] = true; // Włączenie flagi PWM
                } else {
                    // Jeśli PWM nie jest wymagany, ustaw na LOW
                    if (i == 0) {
                        pinMode(OUT_00_PIN, OUTPUT);
                        digitalWrite(OUT_00_PIN, 0);
                    } else if (i == 1) {
                        pinMode(OUT_01_PIN, OUTPUT);
                        digitalWrite(OUT_01_PIN, 0);
                    } else if (i == 2) {
                        pinMode(OUT_02_PIN, OUTPUT);
                        digitalWrite(OUT_02_PIN, 0);
                    } else if (i == 3) {
                        pinMode(OUT_03_PIN, OUTPUT);
                        digitalWrite(OUT_03_PIN, 0);
                    } else if (i == 4) {
                        pinMode(OUT_04_PIN, OUTPUT);
                        digitalWrite(OUT_04_PIN, 0);
                    } else if (i == 5) {
                        pinMode(OUT_05_PIN, OUTPUT);
                        digitalWrite(OUT_05_PIN, 0);
                    }
                }
            }
        }
    }
}

/*==================================================================*/

// Główna pętla dla core 0
void IRAM_ATTR loop_Core0(void* parameter) {
    delay(500); // Opóźnienie początkowe
    Udp.parsePacket(); /* Opróżnianie bufora */

    for (;;) {
        // Zmienna do monitorowania
        static unsigned long ul_watchdog;

        if (Udp.parsePacket() == sizeof(cmd) + 1) { // Sprawdzenie, czy odebrano cały pakiet
            char packetBuffer[60]; // Bufor do odbierania i wysyłania danych

            Udp.read(packetBuffer, sizeof(cmd) + 1); // Odczytanie pakietu

            // Prosta kontrola integralności pakietu
            uint8_t chk = 71; // Prosta suma kontrolna
            for (int i = 0; i < sizeof(cmd); i++) // Bitowe XOR
                chk ^= packetBuffer[i];

            if (packetBuffer[sizeof(cmd)] == chk) { // Sprawdzanie sumy kontrolnej
                memcpy(&cmd, &packetBuffer, sizeof(cmd)); // Kopiowanie danych do struktury
                commandHandler(); // Obsługa komendy
                ul_watchdog = millis(); // Resetowanie watchdog
            }

            inputHandler(); // Sprawdź stan wejść

            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; // Obliczanie prędkości
                else
                    fb.vel[i] = 0.0f; // Ustawienie prędkości na 0
            }

            memcpy(&packetBuffer, &fb, sizeof(fb)); // Kopiowanie danych zwrotnych

            Udp.beginPacket(Udp.remoteIP(), Udp.remotePort()); // Rozpoczęcie pakietu UDP
            Udp.write(packetBuffer, sizeof(fb)); // Wysyłanie bazy danych struktury
            Udp.endPacket(); // Zakończenie pakietu UDP

            outputHandler(); // Obsługuje wyjścia
        }

        // Resetowanie kontrolek po 10 ms
        if (millis() - ul_watchdog > 10ul) {
            fb.control = 0; // Resetowanie kontroli
            cmd.control = 0; // Resetowanie komendy
            outputHandler(); // Zapewnia wyjścia
            ul_watchdog = millis(); // Resetowanie zegara watchdog
        }
    }
}

/*==================================================================*/

// Inicjacja dla core 0
void setup_Core0(void* parameter) {
    // Definiowanie pinów jako wejściowe
    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); // Bez podciągnięcia
    pinMode(IN_05_PIN, INPUT); // Bez podciągnięcia
    pinMode(IN_06_PIN, INPUT); // Bez podciągnięcia
    pinMode(IN_07_PIN, INPUT); // Bez podciągnięcia

    delay(500); // Opóźnienie dla ustabilizowania
    Ethernet.init(SPI_CS_PIN); /* Możesz użyć Ethernet.init(pin) aby skonfigurować pin CS */
    Ethernet.begin(mac, ip); /* Rozpoczęcie Ethernetu */

    delay(500); // Opóźnienie dla ustabilizowania
    Udp.begin(port); /* Rozpoczęcie UDP */

    delay(100); // Opóźnienie na ustabilizowanie systemu
    xTaskCreatePinnedToCore(
        loop_Core0, // Funkcja zadania.
        "loopTask_Core0", // Nazwa zadania.
        4096, // Rozmiar stosu zadania
        NULL, // Parametr zadania
        0, // Priorytet zadania
        NULL, // Uchwyty do zadania
        0); // Przypisanie zadania do core 0

    disableCore0WDT(); // Dezaktywacja watchdog timer

    vTaskDelete(NULL); // Usunięcie zadania (nikogo nie potrzebujemy w tym punkcie)
}

/*==================================================================*/

// Konfiguracja początkowa
void setup() {
    // Inicjalizacja kroku i kierunku dla trzech osi
    pinMode(STEP_0_PIN, OUTPUT);
    digitalWrite(STEP_0_PIN, 0); // Oznacz RTL
    pinMode(DIR_0_PIN, OUTPUT);
    digitalWrite(DIR_0_PIN, 0); // Oznacz RTL
    pinMode(STEP_1_PIN, OUTPUT);
    digitalWrite(STEP_1_PIN, 0); // Oznacz RTL
    pinMode(DIR_1_PIN, OUTPUT);
    digitalWrite(DIR_1_PIN, 0); // Oznacz RTL
    pinMode(STEP_2_PIN, OUTPUT);
    digitalWrite(STEP_2_PIN, 0); // Oznacz RTL
    pinMode(DIR_2_PIN, OUTPUT);
    digitalWrite(DIR_2_PIN, 0); // Oznacz RTL

    // Konfiguracja Prescalera
    // 80 000 000 / 2 = 40 000 000 tics/second ===>   25 ns/tic
    stepGen_0 = timerBegin(0, 2, true); // Rozpoczęcie timera
    stepGen_1 = timerBegin(1, 2, true);
    stepGen_2 = timerBegin(2, 2, true);
    timerAttachInterrupt(stepGen_0, &onTime_0, true); // Ustawienie przerwania dla osi 0
    timerAttachInterrupt(stepGen_1, &onTime_1, true); // Ustawienie przerwania dla osi 1
    timerAttachInterrupt(stepGen_2, &onTime_2, true); // Ustawienie przerwania dla osi 2
    timerAlarmWrite(stepGen_0, 40000000, true); // Alarm dla osi 0
    timerAlarmWrite(stepGen_1, 40000000, true); // Alarm dla osi 1
    timerAlarmWrite(stepGen_2, 40000000, true); // Alarm dla osi 2
    timerAlarmEnable(stepGen_0); // Włączenie alarmu dla osi 0
    timerAlarmEnable(stepGen_1); // Włączenie alarmu dla osi 1
    timerAlarmEnable(stepGen_2); // Włączenie alarmu dla osi 2

    // Tworzenie zadania dla setup_Core0
    xTaskCreatePinnedToCore(
        setup_Core0, // Funkcja zadania.
        "setup_Core0Task", // Nazwa zadania.
        1024, // Rozmiar stosu zadania
        NULL, // Parametr zadania
        0, // Priorytet zadania
        NULL, // Uchwyty do zadania
        0); // Przypisanie zadania do core 0
}

/*==================================================================*/

// Główna pętla sterownika
void IRAM_ATTR loop() {
    for (int i = 0; i < 3; i++) { // Iteracja dla każdej osi
        if (b_math[i]) { // Sprawdzenie, czy obliczenia są potrzebne
            b_math[i] = false; // Reset flagi
            if (!ul_accelStep[i]) { // Oś jest w stanie spoczynku
                long l_pos_error = cmd.pos[i] - fb.pos[i]; // Obliczanie błędu pozycji
                if (l_pos_error) { // Jeśli istnieje błąd pozycji
                    if ((l_pos_error > 0 && b_dirSignal[i] == HIGH) || (l_pos_error < 0 && b_dirSignal[i] == LOW)) // Kierunek jest dobry
                        acceleration(i); // Przyspiesz
                    else { // Musimy zmienić kierunek
                        (l_pos_error > 0) ? b_dirSignal[i] = HIGH : b_dirSignal[i] = LOW; // Ustal kierunek
                        b_dirChange[i] = true; // Ustaw flagę zmiany kierunku
                    }
                }
            } else { // Oś jest w ruchu
                if ((cmd.vel[i] > 0.0f && b_dirSignal[i] == HIGH) || (cmd.vel[i] < 0.0f && b_dirSignal[i] == LOW)) { // Dobry kierunek
                    if (ul_T[i] > ul_cmd_T[i]) // Jeśli prędkość jest zbyt niska
                        acceleration(i); // Przyspiesz
                    else if (ul_T[i] < ul_cmd_T[i]) // Jeśli prędkość jest zbyt wysoka
                        deceleration(i); // Dekceleracja
                } else // Zły kierunek lub celowa prędkość wynosi zero
                    deceleration(i); // Dekceleracja
            }
        }
    }
}
Omówienie kodu:

Header: W początku kodu znajduje się meta-informacja o jego prawach autorskich oraz warunkach użytkowania.
Sprzęt: Osprzęt projektowych dla używanego układu ESP32 i W5500, szczegóły napięcia i pinów między płytką a urządzenia peryferyjnego.
GPIO: Szczegółowe rozpisanie pinów GPIO ESP32, ich zastosowań oraz ich stanów.
Funkcjonalność: Kod pozwala na wysyłanie i odbieranie pakietów przez Ethernet, generowanie kroków w odpowiedzi na dane komendy oraz obsługę PWM. Prostsze obliczenia przyspieszenia oraz zmiany stanu kierunku.
Główna pętla: Obsługuje wykonywanie zadań w iteracyjnym cyklu, jaki jest kluczowy do działania kroków.
Powyższe komentarze powinny pomóc w zrozumieniu działania i struktury kodu w ramach projektu HAL2UDP.
Dodaj link do:
Tweet
drukuj
« powrót
w 24h

HAL2UDP (1) kod z komentarzem

Omówienie kodu:
