From 15645f7d252c0399a4887e6a8de1e64d6235d482 Mon Sep 17 00:00:00 2001 From: drho1y Date: Tue, 7 Jul 2026 05:31:54 +0700 Subject: [PATCH] Switch controlling step driver to serial interface --- arduino_code/Test/README.md | 88 +++---- arduino_code/Test/platformio.ini | 1 + arduino_code/Test/src/config.cpp | 22 +- arduino_code/Test/src/config.h | 21 +- arduino_code/Test/src/main.cpp | 1 + arduino_code/Test/src/motor.cpp | 348 ++++++++++++++++++------- arduino_code/Test/src/motor.h | 37 ++- arduino_code/Test/src/mqtt_handler.cpp | 237 +++++++++-------- 8 files changed, 473 insertions(+), 282 deletions(-) diff --git a/arduino_code/Test/README.md b/arduino_code/Test/README.md index e65fa90..43cc756 100644 --- a/arduino_code/Test/README.md +++ b/arduino_code/Test/README.md @@ -1,66 +1,36 @@ -# MQTT Топики для управления двигателем +# MQTT Топики для управления двигателем и обратной связи -## Общая информация -Система использует MQTT протокол для обмена данными между ESP32 и центральным сервером. Все топики разделены на два типа: -- **Управление** (`motor/control/...`) - для отправки команд -- **Обратная связь** (`motor/feedback/...`) - для получения статуса +## Подписка (от клиента к устройству) -## Управление +### Основное управление +- `motor/control/rpm` - Установка целевой скорости в об/мин (положительное значение - вперед, отрицательное - назад) +- `motor/control/totalsteps/reset` - Сброс счетчика шагов -### 1. Команды управления -- **`motor/control/command`** - - Возможные значения: `start`, `stop` - - Описание: Запуск или остановка двигателя +### Управление TMC (настройки двигателя) +- `motor/control/tmc/current_percent` - Установка текущего тока в процентах (0-100) +- `motor/control/tmc/microsteps` - Установка микрокроков (1, 2, 4, 8, 16, 32, 64, 128, 256) +- `motor/control/tmc/stallguard` - Включение/выключение StallGuard (0-255) +- `motor/control/tmc/enable` - Включение/выключение двигателя ("on"/"off") +- `motor/control/tmc/stealthchop` - Включение/выключение бесшумного режима. ("on"/"off") +- `motor/control/tmc/coolstep` - Включение/выключение режима динамического снижение тока при малой нагрузке("on"/"off") -### 2. RPM (обороты в минуту) -- **`motor/control/rpm`** - - Тип данных: Целое число - - Описание: Установка целевого значения оборотов в минуту +## Публикация (от устройства к клиенту) -### 3. Шаги в секунду -- **`motor/control/step_per_sec`** - - Тип данных: Целое число - - Описание: Установка целевого значения шагов в секунду +### Базовая телеметрия +- `motor/feedback/rpm` - Текущая скорость в об/мин +- `motor/feedback/totalsteps` - Общее количество шагов +- `motor/feedback/is_run` - Статус работы двигателя ("true"/"false") -### 4. Состояние драйвера -- **`motor/control/driver`** - - Возможные значения: `turnon`, `turnoff` - - Описание: Включение или выключение драйвера двигателя +### TMC Телеметрия +- `motor/feedback/tmc/current_percent` - Текущий ток в процентах +- `motor/feedback/tmc/microsteps` - Текущие микрокроки +- `motor/feedback/tmc/sg_result` - Результат StallGuard +- `motor/feedback/tmc/interstep_duration` - Длительность интервала шага (в наносекундах) -### 5. Сброс общего количества шагов -- **`motor/control/totalsteps/reset`** - - Тип данных: Нет полезной нагрузки - - Описание: Сброс общего счетчика шагов - -## Обратная связь - -### 1. RPM (обороты в минуту) -- **`motor/feedback/rpm`** - - Тип данных: Целое число - - Описание: Текущее значение оборотов в минуту - -### 2. Шаги в секунду -- **`motor/feedback/step_per_seconds`** - - Тип данных: Целое число - - Описание: Текущее значение шагов в секунду - -### 3. Общее количество шагов -- **`motor/feedback/totalsteps`** - - Тип данных: Целое число - - Описание: Общее количество выполненных шагов - -### 4. Состояние драйвера -- **`motor/feedback/driver`** - - Возможные значения: `on`, `off` - - Описание: Состояние драйвера (включен/выключен) - -### 5. Состояние работы двигателя -- **`motor/feedback/is_run`** - - Возможные значения: `run`, `stop` - - Описание: Состояние двигателя (работает/стоит) - -## Примечания -- Все числовые значения передаются в строковом формате -- Для управления драйвером используется логический уровень: LOW = включен, HIGH = выключен -- Обратная связь отправляется с интервалом не менее 200 мс -- Система автоматически обрабатывает плавный разгон/торможение при изменении скорости \ No newline at end of file +### Статусы TMC +- `motor/feedback/tmc/status/over_temp` - Предупреждение о перегреве +- `motor/feedback/tmc/status/short_to_ground` - Короткое замыкание на землю +- `motor/feedback/tmc/status/open_load` - Предупреждение о коротком замыкании +- `motor/feedback/tmc/status/stealth_chop_active` - Активен ли бесшумный режим. +- `motor/feedback/tmc/status/standstill` - Стоп-состояние +- `motor/feedback/tmc/status/current_scaling` - Масштабирование тока \ No newline at end of file diff --git a/arduino_code/Test/platformio.ini b/arduino_code/Test/platformio.ini index e232c82..d293586 100644 --- a/arduino_code/Test/platformio.ini +++ b/arduino_code/Test/platformio.ini @@ -17,3 +17,4 @@ upload_speed = 921600 upload_port = /dev/ttyUSB0 lib_deps = knolleary/PubSubClient@^2.8 + janelia-arduino/TMC2209@^9.4.0 diff --git a/arduino_code/Test/src/config.cpp b/arduino_code/Test/src/config.cpp index c887886..a57d148 100644 --- a/arduino_code/Test/src/config.cpp +++ b/arduino_code/Test/src/config.cpp @@ -1,6 +1,5 @@ #include "config.h" -// WiFi & MQTT const char* WIFI_SSID = "Home"; const char* WIFI_PASS = "88888888qwE"; const char* MQTT_SERVER = "192.168.31.225"; @@ -9,12 +8,21 @@ const char* MQTT_USER = "test"; const char* MQTT_PASS = "1234"; const char* MQTT_CLIENT_ID = "ESP32_Stepper"; -// Pins -const int DIR_PIN = 18; -const int STEP_PIN = 19; +// UART2: RX=16, TX=17 +HardwareSerial& TMC_SERIAL = Serial2; +const uint32_t TMC_BAUD_RATE = 115200; +const uint8_t TMC_SERIAL_ADDRESS = 0; // Если MS1 и MS2 на GND + +const int16_t TMC_RX_PIN = 16; +const int16_t TMC_TX_PIN = 17; const int EN_PIN = 21; -// Motor Params const int STEPS_PER_REVOLUTION = 200; -const int MICROSTEP_FACTOR = 1; -const unsigned long RAMP_DURATION_MS = 2000; \ No newline at end of file +const unsigned long RAMP_DURATION_MS = 2000; + +// Ток задается в процентах от максимума (зависит от R_sense). +// Для R_sense=0.11 Ом, 100% ~ 1.77А RMS. Для R_sense=0.15 Ом, 100% ~ 1.2А RMS. +const uint8_t TMC_RUN_CURRENT_PERCENT = 50; // 50% тока при движении +const uint8_t TMC_HOLD_CURRENT_PERCENT = 20; // 20% тока в простое +const uint8_t TMC_STALL_GUARD_THRESH = 10; +const uint16_t TMC_MICROSTEPS = 1; \ No newline at end of file diff --git a/arduino_code/Test/src/config.h b/arduino_code/Test/src/config.h index b435207..6789698 100644 --- a/arduino_code/Test/src/config.h +++ b/arduino_code/Test/src/config.h @@ -1,6 +1,9 @@ #ifndef CONFIG_H #define CONFIG_H +#include +#include + // WiFi & MQTT extern const char* WIFI_SSID; extern const char* WIFI_PASS; @@ -10,14 +13,22 @@ extern const char* MQTT_USER; extern const char* MQTT_PASS; extern const char* MQTT_CLIENT_ID; -// Pins -extern const int DIR_PIN; -extern const int STEP_PIN; +// UART for TMC2209 +extern HardwareSerial& TMC_SERIAL; +extern const uint32_t TMC_BAUD_RATE; +extern const uint8_t TMC_SERIAL_ADDRESS; // Адрес драйвера (0-3) +extern const int16_t TMC_RX_PIN; +extern const int16_t TMC_TX_PIN; extern const int EN_PIN; // Motor Params -extern const int STEPS_PER_REVOLUTION; -extern const int MICROSTEP_FACTOR; +extern const int STEPS_PER_REVOLUTION; // Базовые шаги мотора (обычно 200) extern const unsigned long RAMP_DURATION_MS; +// TMC2209 Defaults (Токи в процентах 0-100%) +extern const uint8_t TMC_RUN_CURRENT_PERCENT; +extern const uint8_t TMC_HOLD_CURRENT_PERCENT; +extern const uint8_t TMC_STALL_GUARD_THRESH; +extern const uint16_t TMC_MICROSTEPS; + #endif \ No newline at end of file diff --git a/arduino_code/Test/src/main.cpp b/arduino_code/Test/src/main.cpp index b643de1..6c5846c 100644 --- a/arduino_code/Test/src/main.cpp +++ b/arduino_code/Test/src/main.cpp @@ -1,4 +1,5 @@ #include +#include #include "config.h" #include "motor.h" #include "mqtt_handler.h" diff --git a/arduino_code/Test/src/motor.cpp b/arduino_code/Test/src/motor.cpp index d5712d3..d727772 100644 --- a/arduino_code/Test/src/motor.cpp +++ b/arduino_code/Test/src/motor.cpp @@ -1,134 +1,282 @@ #include "motor.h" #include "config.h" -#include -volatile bool motor_enabled_logic = false; -volatile unsigned long current_step_interval_us = 100000; +static TMC2209 stepper_driver; +static bool tmc_initialized = false; +static SemaphoreHandle_t tmc_uart_mutex = NULL; + volatile unsigned long total_steps = 0; - static int target_rpm = 0; static int current_rpm_display = 0; -static hw_timer_t *stepTimer = NULL; static bool is_ramping = false; static unsigned long ramp_start_ms = 0; static float start_speed_sps = 0; static float end_speed_sps = 0; +static float current_speed_sps = 0; +static int32_t last_vactual = 0; +static bool velocity_sent = false; // Флаг для отправки хотя бы раз -// --- ISR --- -void IRAM_ATTR onStepTimer() { - if (digitalRead(EN_PIN) == LOW && motor_enabled_logic) { - GPIO.out_w1ts = (1 << STEP_PIN); - ets_delay_us(1); - GPIO.out_w1tc = (1 << STEP_PIN); - total_steps++; - } +static uint16_t current_microsteps = TMC_MICROSTEPS; +static uint8_t current_run_percent = TMC_RUN_CURRENT_PERCENT; + +#define TMC_LOCK() xSemaphoreTake(tmc_uart_mutex, portMAX_DELAY) +#define TMC_UNLOCK() xSemaphoreGive(tmc_uart_mutex) + +const float TMC_FCLK = 12800000.0; +const float VACTUAL_FACTOR = 8388608.0 / TMC_FCLK; + +int32_t calculateVActual(float microsteps_per_second) { + return (int32_t)(microsteps_per_second * VACTUAL_FACTOR); } -unsigned long getStepIntervalUs() { - return current_step_interval_us; -} +void motorInit() { + pinMode(EN_PIN, OUTPUT); + digitalWrite(EN_PIN, LOW); + tmc_uart_mutex = xSemaphoreCreateMutex(); -// --- Private Helpers --- -static void updateRamp() { - unsigned long now = millis(); - unsigned long elapsed = now - ramp_start_ms; - - if (elapsed >= RAMP_DURATION_MS) { - is_ramping = false; - current_step_interval_us = (end_speed_sps > 0) ? (1000000UL / (unsigned long)end_speed_sps) : 100000; - - if (end_speed_sps <= 0) { - motor_enabled_logic = false; - } else { - motor_enabled_logic = true; - } - - timerAlarmWrite(stepTimer, current_step_interval_us, true); + TMC_SERIAL.begin(TMC_BAUD_RATE, SERIAL_8N1, TMC_RX_PIN, TMC_TX_PIN); + delay(500); + + TMC_LOCK(); + + stepper_driver.setup(TMC_SERIAL, TMC_BAUD_RATE, + (TMC2209::SerialAddress)TMC_SERIAL_ADDRESS, + TMC_RX_PIN, TMC_TX_PIN); + + delay(200); + + // Проверка связи + if (!stepper_driver.isCommunicating()) { + Serial.println("ERROR: TMC2209 not communicating!"); + TMC_UNLOCK(); + tmc_initialized = false; return; } - - float progress = (float)elapsed / RAMP_DURATION_MS; - float current_sps = start_speed_sps + (end_speed_sps - start_speed_sps) * progress; - if (current_sps < 1) current_sps = 1; - - unsigned long new_interval = 1000000UL / (unsigned long)current_sps; - if (new_interval < 10) new_interval = 10; - - current_step_interval_us = new_interval; - timerAlarmWrite(stepTimer, new_interval, true); - - unsigned long total_steps_per_rev = (unsigned long)STEPS_PER_REVOLUTION * MICROSTEP_FACTOR; - current_rpm_display = ((unsigned long)current_sps * 60) / total_steps_per_rev; -} - -// --- Public API --- -void motorInit() { - pinMode(DIR_PIN, OUTPUT); - pinMode(STEP_PIN, OUTPUT); - pinMode(EN_PIN, OUTPUT); - digitalWrite(EN_PIN, HIGH); - digitalWrite(DIR_PIN, HIGH); - digitalWrite(STEP_PIN, LOW); + Serial.println("TMC2209 communicating OK"); + + // Базовая настройка + stepper_driver.setMicrostepsPerStep(TMC_MICROSTEPS); + stepper_driver.setRunCurrent(TMC_RUN_CURRENT_PERCENT); + stepper_driver.setHoldCurrent(TMC_HOLD_CURRENT_PERCENT); + stepper_driver.setHoldDelay(7); + stepper_driver.setStallGuardThreshold(TMC_STALL_GUARD_THRESH); + + stepper_driver.enableAutomaticCurrentScaling(); + stepper_driver.enableAutomaticGradientAdaptation(); + + // КРИТИЧНО: Отключаем StealthChop для работы moveAtVelocity()! + stepper_driver.disableStealthChop(); + delay(10); + + // Включаем CoolStep для энергосбережения + stepper_driver.enableCoolStep(); + + // Программное включение драйвера + stepper_driver.enable(); + delay(100); + + tmc_initialized = stepper_driver.isSetupAndCommunicating(); + + if (tmc_initialized) { + Serial.println("TMC2209 initialized successfully"); + TMC2209::Settings settings = stepper_driver.getSettings(); + Serial.printf("Run: %d%%, Hold: %d%%, Microsteps: %d, StealthChop: %s\n", + settings.irun_percent, settings.ihold_percent, + settings.microsteps_per_step, settings.stealth_chop_enabled ? "ON" : "OFF"); + } else { + Serial.println("ERROR: TMC2209 setup failed!"); + } + + TMC_UNLOCK(); - stepTimer = timerBegin(0, 80, true); - timerAttachInterrupt(stepTimer, &onStepTimer, true); - timerAlarmWrite(stepTimer, 100000, true); - timerAlarmEnable(stepTimer); + current_microsteps = TMC_MICROSTEPS; + current_run_percent = TMC_RUN_CURRENT_PERCENT; } void motorLoop() { if (is_ramping) { - updateRamp(); + unsigned long now = millis(); + unsigned long elapsed = now - ramp_start_ms; + + if (elapsed >= RAMP_DURATION_MS) { + is_ramping = false; + current_speed_sps = end_speed_sps; + } else { + float progress = (float)elapsed / RAMP_DURATION_MS; + current_speed_sps = start_speed_sps + (end_speed_sps - start_speed_sps) * progress; + } + + int32_t vactual = calculateVActual(current_speed_sps); + + // Отправляем если: скорость изменилась ИЛИ это первая отправка в рампе + if (abs(vactual - last_vactual) >= 0 || !velocity_sent) { + TMC_LOCK(); + stepper_driver.moveAtVelocity(vactual); + TMC_UNLOCK(); + last_vactual = vactual; + velocity_sent = true; + + Serial.printf("VACTUAL: %d (SPS: %.1f, RPM: %d)\n", + vactual, current_speed_sps, current_rpm_display); + } + + unsigned long steps_per_rev = (unsigned long)STEPS_PER_REVOLUTION * current_microsteps; + current_rpm_display = ((unsigned long)abs(current_speed_sps) * 60) / steps_per_rev; + } else if (!velocity_sent && current_speed_sps == 0) { + // Если мотор стоит и скорость не отправлялась - отправляем 0 + TMC_LOCK(); + stepper_driver.moveAtVelocity(0); + TMC_UNLOCK(); + velocity_sent = true; } -} - -void resetSteps() { - taskDISABLE_INTERRUPTS(); - total_steps = 0; - taskENABLE_INTERRUPTS(); -} - -unsigned long getMotorSteps() { - unsigned long steps; - taskDISABLE_INTERRUPTS(); - steps = total_steps; - taskENABLE_INTERRUPTS(); - return steps; -} - -int getCurrentRPM() { - return current_rpm_display; -} - -bool isMotorRunning() { - return (motor_enabled_logic && current_step_interval_us < 100000); + if (current_speed_sps != 0) { + total_steps += (unsigned long)(abs(current_speed_sps) * 0.005); + } + + vTaskDelay(pdMS_TO_TICKS(5)); } void setTargetRPM(int rpm) { target_rpm = rpm; - - // Сброс шагов при старте с нуля - if (rpm > 0 && current_rpm_display < 5) { - resetSteps(); - } + if (rpm != 0 && current_rpm_display < 5) resetSteps(); - unsigned long total_steps_per_rev = (unsigned long)STEPS_PER_REVOLUTION * MICROSTEP_FACTOR; + unsigned long steps_per_rev = (unsigned long)STEPS_PER_REVOLUTION * current_microsteps; - if (current_step_interval_us > 0 && current_step_interval_us < 100000) { - start_speed_sps = 1000000.0f / current_step_interval_us; - } else { - start_speed_sps = 0; - } - - if (rpm > 0) { - end_speed_sps = ((float)rpm * total_steps_per_rev) / 60.0f; - } else { - end_speed_sps = 0; - } + start_speed_sps = current_speed_sps; + end_speed_sps = (rpm != 0) ? ((float)rpm * steps_per_rev) / 60.0f : 0; ramp_start_ms = millis(); is_ramping = true; - motor_enabled_logic = true; + velocity_sent = false; // Сбрасываем флаг для новой отправки + + Serial.printf("Target RPM: %d -> SPS: %.1f\n", rpm, end_speed_sps); +} + +void resetSteps() { + total_steps = 0; +} + +unsigned long getMotorSteps() { return total_steps; } +int getCurrentRPM() { return current_rpm_display; } +bool isMotorRunning() { return (abs(current_speed_sps) > 1); } + +void tmcSetCurrentPercent(uint8_t run_percent, uint8_t hold_percent) { + if (!tmc_initialized) return; + TMC_LOCK(); + stepper_driver.setAllCurrentValues(run_percent, hold_percent, 7); + TMC_UNLOCK(); + current_run_percent = run_percent; +} + +void tmcSetMicrosteps(uint16_t ms) { + if (!tmc_initialized) return; + TMC_LOCK(); + stepper_driver.setMicrostepsPerStep(ms); + TMC_UNLOCK(); + current_microsteps = ms; + if (target_rpm != 0) setTargetRPM(target_rpm); +} + +void tmcSetStallGuard(uint8_t threshold) { + if (!tmc_initialized) return; + TMC_LOCK(); + stepper_driver.setStallGuardThreshold(threshold); + TMC_UNLOCK(); +} + +void tmcSoftwareEnable(bool enable) { + if (!tmc_initialized) return; + TMC_LOCK(); + if (enable) { + stepper_driver.enable(); + // После enable нужно заново отправить скорость + velocity_sent = false; + } else { + stepper_driver.moveAtVelocity(0); + stepper_driver.disable(); + last_vactual = 0; + current_speed_sps = 0; + is_ramping = false; + } + TMC_UNLOCK(); +} + +void tmcSetStealthChop(bool enable) { + if (!tmc_initialized) return; + TMC_LOCK(); + if (enable) { + stepper_driver.enableStealthChop(); + // В StealthChop VACTUAL не работает, останавливаем мотор + stepper_driver.moveAtVelocity(0); + last_vactual = 0; + current_speed_sps = 0; + } else { + stepper_driver.disableStealthChop(); + velocity_sent = false; + } + TMC_UNLOCK(); +} + +void tmcSetCoolStep(bool enable) { + if (!tmc_initialized) return; + TMC_LOCK(); + enable ? stepper_driver.enableCoolStep() : stepper_driver.disableCoolStep(); + TMC_UNLOCK(); +} + +bool tmcIsInitialized() { return tmc_initialized; } + +bool tmcIsCommunicating() { + if (!tmc_initialized) return false; + TMC_LOCK(); + bool res = stepper_driver.isCommunicating(); + TMC_UNLOCK(); + return res; +} + +uint16_t tmcGetMicrostepsSetting() { return current_microsteps; } +uint8_t tmcGetRunCurrentPercent() { return current_run_percent; } + +TMC2209::Status tmcGetStatus() { + TMC2209::Status s = {}; + if (!tmc_initialized) return s; + TMC_LOCK(); + s = stepper_driver.getStatus(); + TMC_UNLOCK(); + return s; +} + +TMC2209::Settings tmcGetSettings() { + TMC2209::Settings s = {}; + if (!tmc_initialized) return s; + TMC_LOCK(); + s = stepper_driver.getSettings(); + TMC_UNLOCK(); + return s; +} + +uint16_t tmcGetStallGuardResult() { + if (!tmc_initialized) return 0; + TMC_LOCK(); + uint16_t res = stepper_driver.getStallGuardResult(); + TMC_UNLOCK(); + return res; +} + +uint32_t tmcGetInterstepDuration() { + if (!tmc_initialized) return 0; + TMC_LOCK(); + uint32_t res = stepper_driver.getInterstepDuration(); + TMC_UNLOCK(); + return res; +} + +uint16_t tmcGetMicrostepCounter() { + if (!tmc_initialized) return 0; + TMC_LOCK(); + uint16_t res = stepper_driver.getMicrostepCounter(); + TMC_UNLOCK(); + return res; } \ No newline at end of file diff --git a/arduino_code/Test/src/motor.h b/arduino_code/Test/src/motor.h index 01a5488..0869f29 100644 --- a/arduino_code/Test/src/motor.h +++ b/arduino_code/Test/src/motor.h @@ -2,14 +2,39 @@ #define MOTOR_H #include +#include void motorInit(); -void setTargetRPM(int rpm); -void resetSteps(); -unsigned long getMotorSteps(); -int getCurrentRPM(); -bool isMotorRunning(); -unsigned long getStepIntervalUs(); void motorLoop(); +// Управление движением (через UART VACTUAL) +void setTargetRPM(int rpm); // Поддерживает отрицательные значения для реверса! +void resetSteps(); + +// Геттеры состояния движения +unsigned long getMotorSteps(); // Считается программно +int getCurrentRPM(); +bool isMotorRunning(); + +// Управление TMC2209 через UART +void tmcSetCurrentPercent(uint8_t run_percent, uint8_t hold_percent); +void tmcSetMicrosteps(uint16_t ms); +void tmcSetStallGuard(uint8_t threshold); +void tmcSoftwareEnable(bool enable); // Вкл/Выкл драйвер программно +void tmcSetStealthChop(bool enable); +void tmcSetCoolStep(bool enable); + +// Расширенная телеметрия +bool tmcIsInitialized(); +bool tmcIsCommunicating(); +uint16_t tmcGetMicrostepsSetting(); +uint8_t tmcGetRunCurrentPercent(); + +// Структуры статусов для MQTT +TMC2209::Status tmcGetStatus(); +TMC2209::Settings tmcGetSettings(); +uint16_t tmcGetStallGuardResult(); +uint32_t tmcGetInterstepDuration(); +uint16_t tmcGetMicrostepCounter(); + #endif \ No newline at end of file diff --git a/arduino_code/Test/src/mqtt_handler.cpp b/arduino_code/Test/src/mqtt_handler.cpp index 78cc4dc..e077c8f 100644 --- a/arduino_code/Test/src/mqtt_handler.cpp +++ b/arduino_code/Test/src/mqtt_handler.cpp @@ -4,90 +4,70 @@ #include #include -// --- ГЛОБАЛЬНЫЕ ОБЪЕКТЫ (Static внутри cpp) --- static WiFiClient espClient; static PubSubClient client(espClient); -// --- ПЕРЕМЕННЫЕ ДЛЯ ОТСЛЕЖИВАНИЯ ИЗМЕНЕНИЙ ТЕЛЕМЕТРИИ --- +// Кэш телеметрии static unsigned long last_feedback_time = 0; static int last_pub_rpm = -1; -static unsigned long last_pub_sps = -1; static unsigned long last_pub_steps = -1; -static int last_pub_driver_state = -1; // -1: unknown, 0: off, 1: on -static int last_pub_is_run = -1; // -1: unknown, 0: stop, 1: run +static int last_pub_is_run = -1; -// --- ВСПОМОГАТЕЛЬНЫЕ ФУНКЦИИ --- +// TMC Кэш +static uint16_t last_pub_sg = 65535; +static uint32_t last_pub_interstep = 0; +static uint8_t last_pub_current_pct = 255; +static uint16_t last_pub_microsteps = 0; + +// Статусы (битовые флаги) +static int last_pub_over_temp = -1; +static int last_pub_short_gnd = -1; +static int last_pub_open_load = -1; +static int last_pub_stealth_active = -1; +static int last_pub_standstill = -1; +static uint8_t last_pub_current_scaling = 255; static void setup_wifi() { Serial.print("Connecting to WiFi"); WiFi.begin(WIFI_SSID, WIFI_PASS); - while (WiFi.status() != WL_CONNECTED) { - vTaskDelay(pdMS_TO_TICKS(500)); - Serial.print("."); - } + while (WiFi.status() != WL_CONNECTED) { vTaskDelay(pdMS_TO_TICKS(500)); Serial.print("."); } Serial.println("\nWiFi Connected"); } static void reconnect() { while (!client.connected()) { - Serial.print("Attempting MQTT connection..."); if (client.connect(MQTT_CLIENT_ID, MQTT_USER, MQTT_PASS)) { - Serial.println("connected"); - - // Сбрасываем кэш телеметрии, чтобы при переподключении - // сразу отправить актуальное состояние, даже если оно не менялось - last_pub_rpm = -1; - last_pub_sps = -1; - last_pub_steps = -1; - last_pub_driver_state = -1; - last_pub_is_run = -1; + // Сброс кэша + last_pub_rpm = -1; last_pub_steps = -1; last_pub_is_run = -1; + last_pub_sg = 65535; last_pub_interstep = 0; last_pub_current_pct = 255; + last_pub_microsteps = 0; last_pub_over_temp = -1; last_pub_short_gnd = -1; + last_pub_open_load = -1; last_pub_stealth_active = -1; last_pub_standstill = -1; + last_pub_current_scaling = 255; - // Подписка на топики управления - client.subscribe("motor/control/command"); - client.subscribe("motor/control/driver"); - client.subscribe("motor/control/step_per_sec"); + // Подписки client.subscribe("motor/control/rpm"); + client.subscribe("motor/control/driver"); client.subscribe("motor/control/totalsteps/reset"); + + client.subscribe("motor/control/tmc/current_percent"); + client.subscribe("motor/control/tmc/microsteps"); + client.subscribe("motor/control/tmc/stallguard"); + client.subscribe("motor/control/tmc/enable"); + client.subscribe("motor/control/tmc/stealthchop"); + client.subscribe("motor/control/tmc/coolstep"); } else { - Serial.printf("failed, rc=%d, retry in 5s\n", client.state()); vTaskDelay(pdMS_TO_TICKS(5000)); } } } static void callback(char* topic, byte* payload, unsigned int length) { - // Безопасное преобразование payload в строку char msg[length + 1]; memcpy(msg, payload, length); msg[length] = '\0'; - Serial.printf("MQTT Received [%s]: %s\n", topic, msg); - - if (strcmp(topic, "motor/control/command") == 0) { - if (strcmp(msg, "start") == 0) { - // Если была команда stop (RPM 0), запускаем на дефолтной скорости или последней целевой - // Здесь для простоты запускаем на 60 RPM, если текущая скорость близка к 0 - if (getCurrentRPM() < 5) { - setTargetRPM(60); - } else { - // Если мотор уже крутится, можно просто возобновить рампу к текущей цели, - // но setTargetRPM и так это делает. - setTargetRPM(getCurrentRPM()); - } - } else if (strcmp(msg, "stop") == 0) { - setTargetRPM(0); - } - } - else if (strcmp(topic, "motor/control/rpm") == 0) { - setTargetRPM(atoi(msg)); - } - else if (strcmp(topic, "motor/control/step_per_sec") == 0) { - unsigned long sps = strtoul(msg, NULL, 10); - unsigned long total_steps_per_rev = (unsigned long)STEPS_PER_REVOLUTION * MICROSTEP_FACTOR; - if (sps > 0) { - int calc_rpm = (sps * 60) / total_steps_per_rev; - setTargetRPM(calc_rpm); - } + if (strcmp(topic, "motor/control/rpm") == 0) { + setTargetRPM(atoi(msg)); // Поддерживает отрицательные для реверса! } else if (strcmp(topic, "motor/control/driver") == 0) { // TMC2209: LOW = Enabled, HIGH = Disabled @@ -96,85 +76,132 @@ static void callback(char* topic, byte* payload, unsigned int length) { } else if (strcmp(topic, "motor/control/totalsteps/reset") == 0) { resetSteps(); - // Принудительно публикуем 0 сразу, не дожидаясь цикла телеметрии - if (client.connected()) { - client.publish("motor/feedback/totalsteps", "0"); - } + if (client.connected()) client.publish("motor/feedback/totalsteps", "0"); last_pub_steps = 0; } + // --- TMC Control --- + else if (strcmp(topic, "motor/control/tmc/current_percent") == 0) { + uint8_t pct = atoi(msg); + if (pct <= 100) tmcSetCurrentPercent(pct, pct / 2); // Hold = 50% от Run + } + else if (strcmp(topic, "motor/control/tmc/microsteps") == 0) { + uint16_t ms = atoi(msg); + tmcSetMicrosteps(ms); + } + else if (strcmp(topic, "motor/control/tmc/stallguard") == 0) { + tmcSetStallGuard(atoi(msg)); + } + else if (strcmp(topic, "motor/control/tmc/enable") == 0) { + tmcSoftwareEnable(strcmp(msg, "on") == 0); + } + else if (strcmp(topic, "motor/control/tmc/stealthchop") == 0) { + tmcSetStealthChop(strcmp(msg, "on") == 0); + } + else if (strcmp(topic, "motor/control/tmc/coolstep") == 0) { + tmcSetCoolStep(strcmp(msg, "on") == 0); + } } static void publishTelemetry() { unsigned long now = millis(); - - // Проверяем каждые 200 мс - if (now - last_feedback_time >= 200) { + if (now - last_feedback_time >= 500) { // Читаем статусы раз в 500мс - // 1. RPM - int current_rpm = getCurrentRPM(); - if (current_rpm != last_pub_rpm) { - client.publish("motor/feedback/rpm", String(current_rpm).c_str()); - last_pub_rpm = current_rpm; + // Базовая телеметрия + int rpm = getCurrentRPM(); + if (rpm != last_pub_rpm) { + client.publish("motor/feedback/rpm", String(rpm).c_str()); + last_pub_rpm = rpm; } - // 2. Steps Per Second (SPS) - unsigned long interval_us = getStepIntervalUs(); - unsigned long current_sps = 0; - if (interval_us > 0 && interval_us < 100000) { - current_sps = 1000000UL / interval_us; - } - - if (current_sps != last_pub_sps) { - client.publish("motor/feedback/step_per_seconds", String(current_sps).c_str()); - last_pub_sps = current_sps; + unsigned long steps = getMotorSteps(); + if (steps != last_pub_steps) { + client.publish("motor/feedback/totalsteps", String(steps).c_str()); + last_pub_steps = steps; } - // 3. Total Steps - unsigned long current_steps = getMotorSteps(); - if (current_steps != last_pub_steps) { - client.publish("motor/feedback/totalsteps", String(current_steps).c_str()); - last_pub_steps = current_steps; + int run = isMotorRunning() ? 1 : 0; + if (run != last_pub_is_run) { + client.publish("motor/feedback/is_run", run ? "true" : "false"); + last_pub_is_run = run; } - // 4. Driver State (EN_PIN) - // LOW = On, HIGH = Off - int current_driver_state = (digitalRead(EN_PIN) == LOW) ? 1 : 0; - if (current_driver_state != last_pub_driver_state) { - client.publish("motor/feedback/driver", current_driver_state ? "on" : "off"); - last_pub_driver_state = current_driver_state; - } + // TMC Телеметрия + if (tmcIsInitialized()) { + uint8_t pct = tmcGetRunCurrentPercent(); + if (pct != last_pub_current_pct) { + client.publish("motor/feedback/tmc/current_percent", String(pct).c_str()); + last_pub_current_pct = pct; + } - // 5. Is Running - int current_is_run = isMotorRunning() ? 1 : 0; - if (current_is_run != last_pub_is_run) { - client.publish("motor/feedback/is_run", current_is_run ? "true" : "false"); - last_pub_is_run = current_is_run; - } + uint16_t ms = tmcGetMicrostepsSetting(); + if (ms != last_pub_microsteps) { + client.publish("motor/feedback/tmc/microsteps", String(ms).c_str()); + last_pub_microsteps = ms; + } + uint16_t sg = tmcGetStallGuardResult(); + if (sg != last_pub_sg) { + client.publish("motor/feedback/tmc/sg_result", String(sg).c_str()); + last_pub_sg = sg; + } + + uint32_t interstep = tmcGetInterstepDuration(); + if (interstep != last_pub_interstep) { + client.publish("motor/feedback/tmc/interstep_duration", String(interstep).c_str()); + last_pub_interstep = interstep; + } + + // Чтение полного статуса (требует чтения регистра DRV_STATUS) + TMC2209::Status status = tmcGetStatus(); + + int ot = (status.over_temperature_warning || status.over_temperature_shutdown) ? 1 : 0; + if (ot != last_pub_over_temp) { + client.publish("motor/feedback/tmc/status/over_temp", ot ? "true" : "false"); + last_pub_over_temp = ot; + } + + int sgnd = (status.short_to_ground_a || status.short_to_ground_b) ? 1 : 0; + if (sgnd != last_pub_short_gnd) { + client.publish("motor/feedback/tmc/status/short_to_ground", sgnd ? "true" : "false"); + last_pub_short_gnd = sgnd; + } + + int ol = (status.open_load_a || status.open_load_b) ? 1 : 0; + if (ol != last_pub_open_load) { + client.publish("motor/feedback/tmc/status/open_load", ol ? "true" : "false"); + last_pub_open_load = ol; + } + + int sa = status.stealth_chop_mode ? 1 : 0; + if (sa != last_pub_stealth_active) { + client.publish("motor/feedback/tmc/status/stealth_chop_active", sa ? "true" : "false"); + last_pub_stealth_active = sa; + } + + int ss = status.standstill ? 1 : 0; + if (ss != last_pub_standstill) { + client.publish("motor/feedback/tmc/status/standstill", ss ? "true" : "false"); + last_pub_standstill = ss; + } + + if (status.current_scaling != last_pub_current_scaling) { + client.publish("motor/feedback/tmc/status/current_scaling", String(status.current_scaling).c_str()); + last_pub_current_scaling = status.current_scaling; + } + } last_feedback_time = now; } } -// --- ОСНОВНАЯ ЗАДАЧА FREE RTOS --- void mqttTask(void *parameter) { - // Инициализация WiFi и MQTT клиента setup_wifi(); client.setServer(MQTT_SERVER, MQTT_PORT); client.setCallback(callback); for (;;) { - // Поддержание соединения - if (!client.connected()) { - reconnect(); - } - - // Обработка входящих сообщений + if (!client.connected()) reconnect(); client.loop(); - - // Отправка телеметрии publishTelemetry(); - - // Yield для других задач vTaskDelay(pdMS_TO_TICKS(10)); } } \ No newline at end of file