From 023e6997712729af1723b63fa52078064acd518e Mon Sep 17 00:00:00 2001 From: drho1y Date: Tue, 7 Jul 2026 21:54:03 +0700 Subject: [PATCH] New: adding control servo-drive --- arduino_code/Test/README.md | 108 +++++++++++++++++++++--- arduino_code/Test/platformio.ini | 1 + arduino_code/Test/src/main.cpp | 2 +- arduino_code/Test/src/mqtt_handler.cpp | 60 +++++++++++++ arduino_code/Test/src/servo_control.cpp | 93 ++++++++++++++++++++ arduino_code/Test/src/servo_control.h | 21 +++++ 6 files changed, 270 insertions(+), 15 deletions(-) create mode 100644 arduino_code/Test/src/servo_control.cpp create mode 100644 arduino_code/Test/src/servo_control.h diff --git a/arduino_code/Test/README.md b/arduino_code/Test/README.md index 8cd70e7..ebc744a 100644 --- a/arduino_code/Test/README.md +++ b/arduino_code/Test/README.md @@ -1,21 +1,24 @@ -# Документация MQTT-интерфейса управления шаговым двигателем (TMC2209) +# Документация MQTT-интерфейса управления двигателями (TMC2209 + Сервопривод) -Данный документ описывает структуру MQTT-топиков для удаленного управления шаговым двигателем и получения телеметрии с драйвера TMC2209. +Данный документ описывает структуру MQTT-топиков для удаленного управления шаговым двигателем с драйвером TMC2209 и сервоприводом, а также получения телеметрии с обоих устройств. -**Особенности реализации:** +**Общие особенности реализации:** * **Интервал опроса:** Чтение и публикация статусов происходят каждые **500 мс**. * **State-Change Publishing:** Для экономии трафика и снижения нагрузки на брокер, устройство публикует данные в топики телеметрии **только при изменении значения**. * **Сброс кэша:** При успешном переподключении к MQTT-брокеру внутренний кэш значений сбрасывается, что гарантирует отправку актуального состояния всех параметров сразу после установки связи. +* **Единый контроллер:** Оба устройства управляются через один ESP32 с общей MQTT-инфраструктурой. --- ## 1. Топики управления (Subscribe) Устройство подписано на эти топики для получения команд от пользователя или вышестоящей системы (Home Assistant, Node-RED и т.д.). +### 1.1. Управление шаговым двигателем + | Топик | Тип данных | Описание | Пример Payload | | :--- | :--- | :--- | :--- | | `motor/control/rpm` | `int` | Установка целевой скорости вращения (об/мин). Отрицательные значения включают **реверс**. | `150` (вперед)
`-50` (назад) | -| `motor/control/driver` | `string` | Аппаратное управление пином `EN` драйвера. | `on` (включить)
*Любое другое* (выключить) | +| `motor/control/driver` | `string` | Аппаратное управление пином `EN` драйвера. | `on` (включить)
`off` (выключить) | | `motor/control/totalsteps/reset` | `any` | Сброс счетчика шагов в ноль. После сброса устройство опубликует `0` в `motor/feedback/totalsteps`. | `1` / `reset` | | `motor/control/tmc/current_percent` | `int (0-100)` | Установка тока двигателя (Run Current) в процентах. Ток удержания (Hold) автоматически устанавливается как 50% от заданного. | `80` | | `motor/control/tmc/microsteps` | `int` | Настройка режима микрошага. | `256`, `64`, `16`, `4` | @@ -24,12 +27,21 @@ | `motor/control/tmc/stealthchop` | `string` | Включение/выключение режима StealthChop (бесшумная работа на малых скоростях). | `on` / `off` | | `motor/control/tmc/coolstep` | `string` | Включение/выключение режима CoolStep (адаптивное снижение тока для уменьшения нагрева). | `on` / `off` | +### 1.2. Управление сервоприводом + +| Топик | Тип данных | Описание | Пример Payload | +| :--- | :--- | :--- | :--- | +| `servo/control/angle` | `int (0-180)` | Установка угла поворота сервопривода в градусах. | `90` (центр)
`0` (мин)
`180` (макс) | +| `servo/control/pulse` | `int (500-2500)` | Прямая установка ширины управляющего импульса в микросекундах. | `1500` (центр)
`500` (мин) | +| `servo/control/enable` | `string` | Включение/выключение сервопривода. При `off` происходит detach (снятие сигнала). | `on` / `off` | +| `servo/control/detach` | `string` | Принудительное отсоединение сигнала (серво перестаёт держать позицию, экономия энергии). | `1` / `true` | + --- ## 2. Топики телеметрии (Publish) -Устройство публикует в эти топики текущее состояние двигателя и драйвера. +Устройство публикует в эти топики текущее состояние двигателей и драйверов. -### 2.1. Базовая телеметрия двигателя +### 2.1. Базовая телеметрия шагового двигателя | Топик | Тип данных | Описание | Пример Payload | | :--- | :--- | :--- | :--- | | `motor/feedback/rpm` | `int` | Текущая фактическая скорость вращения (об/мин). Может быть отрицательной при реверсе. | `145` | @@ -55,18 +67,22 @@ | `motor/feedback/tmc/status/stealth_chop_active` | `bool (string)` | Находится ли драйвер в режиме StealthChop в данный момент. | `true` / `false` | | `motor/feedback/tmc/status/standstill` | `bool (string)` | Находится ли двигатель в состоянии покоя (Standstill). | `true` / `false` | | `motor/feedback/tmc/status/current_scaling` | `uint8_t` | Текущий коэффициент масштабирования тока (Current Scaling), применяемый драйвером. | `150` | +| `motor/feedback/driver/status` | `string` | Общий результат аппаратной проверки драйвера. | `on` / `off` | +| `motor/feedback/tmc/status` | `string` | Результат проверки программного флага включения TMC. | `on` / `off` | -### 2.4. Общие статусы системы +### 2.4. Телеметрия сервопривода | Топик | Тип данных | Описание | Пример Payload | | :--- | :--- | :--- | :--- | -| `motor/feedback/driver/status` | `string` | Общий результат аппаратной проверки драйвера (`checkDriverStatus()`). | `on` / `off` | -| `motor/feedback/tmc/status` | `string` | Результат проверки программного флага включения TMC (`checkTmcSoftwareEnable()`). | `on` / `off` | +| `servo/feedback/angle` | `int` | Текущий установленный угол поворота (0-180). | `90` | +| `servo/feedback/status` | `string` | Статус сервопривода (включен/выключен). | `on` / `off` | --- ## 3. Примеры сценариев использования -### Запуск двигателя с нужными параметрами +### 3.1. Управление шаговым двигателем + +#### Запуск двигателя с нужными параметрами ```bash # Устанавливаем ток 70% и микрошаг 16 mosquitto_pub -t "motor/control/tmc/current_percent" -m "70" @@ -76,11 +92,11 @@ mosquitto_pub -t "motor/control/tmc/microsteps" -m "16" mosquitto_pub -t "motor/control/tmc/stealthchop" -m "on" # Аппаратно включаем драйвер и задаем скорость 100 RPM -mosquitto_pub -t "motor/control/driver" -m "turnon" +mosquitto_pub -t "motor/control/driver" -m "on" mosquitto_pub -t "motor/control/rpm" -m "100" ``` -### Аварийная остановка и сброс +#### Аварийная остановка и сброс ```bash # Остановка (скорость 0) mosquitto_pub -t "motor/control/rpm" -m "0" @@ -92,5 +108,69 @@ mosquitto_pub -t "motor/control/tmc/enable" -m "off" mosquitto_pub -t "motor/control/totalsteps/reset" -m "1" ``` -### Мониторинг нагрузки (StallGuard) -Для отслеживания нагрузки на вал или момента срыва шагов, необходимо слушать топик `motor/feedback/tmc/sg_result`. Резкое падение значения `sg_result` при движении обычно означает столкновение или заклинивание механизма. \ No newline at end of file +#### Мониторинг нагрузки (StallGuard) +Для отслеживания нагрузки на вал или момента срыва шагов, необходимо слушать топик `motor/feedback/tmc/sg_result`. Резкое падение значения `sg_result` при движении обычно означает столкновение или заклинивание механизма. + +### 3.2. Управление сервоприводом + +#### Базовое позиционирование +```bash +# Установить серво в центральное положение (90°) +mosquitto_pub -t "servo/control/angle" -m "90" + +# Установить в крайнее положение (180°) +mosquitto_pub -t "servo/control/angle" -m "180" + +# Вернуть в ноль +mosquitto_pub -t "servo/control/angle" -m "0" +``` + +#### Плавное движение (bash-скрипт) +```bash +#!/bin/bash +for angle in 0 45 90 135 180 135 90 45 0; do + mosquitto_pub -t "servo/control/angle" -m "$angle" + sleep 1 +done +``` + +#### Точное управление по ширине импульса +```bash +# Установка точного положения через ширину импульса (1500 мкс = центр) +mosquitto_pub -t "servo/control/pulse" -m "1500" + +# Минимальное положение +mosquitto_pub -t "servo/control/pulse" -m "500" + +# Максимальное положение +mosquitto_pub -t "servo/control/pulse" -m "2500" +``` + +#### Управление питанием серво +```bash +# Отключить серво (снять удержание, экономия энергии) +mosquitto_pub -t "servo/control/enable" -m "off" + +# Включить серво (восстановить удержание позиции) +mosquitto_pub -t "servo/control/enable" -m "on" + +# Принудительное отсоединение сигнала +mosquitto_pub -t "servo/control/detach" -m "1" +``` + +### 3.3. Комплексные сценарии + +#### Одновременное управление обоими двигателями +```bash +# Запуск шагового двигателя на 50 RPM +mosquitto_pub -t "motor/control/rpm" -m "50" + +# Одновременное перемещение серво в положение 45° +mosquitto_pub -t "servo/control/angle" -m "45" + +# Мониторинг телеметрии обоих устройств +mosquitto_sub -t "motor/feedback/#" -t "servo/feedback/#" +``` + +--- + diff --git a/arduino_code/Test/platformio.ini b/arduino_code/Test/platformio.ini index d293586..01ee9e0 100644 --- a/arduino_code/Test/platformio.ini +++ b/arduino_code/Test/platformio.ini @@ -18,3 +18,4 @@ upload_port = /dev/ttyUSB0 lib_deps = knolleary/PubSubClient@^2.8 janelia-arduino/TMC2209@^9.4.0 + madhephaestus/ESP32Servo@^3.2.1 diff --git a/arduino_code/Test/src/main.cpp b/arduino_code/Test/src/main.cpp index 6c5846c..5212cd6 100644 --- a/arduino_code/Test/src/main.cpp +++ b/arduino_code/Test/src/main.cpp @@ -14,7 +14,7 @@ void setup() { xTaskCreatePinnedToCore( mqttTask, "MQTT_Task", - 8192, + 20480, NULL, 3, NULL, diff --git a/arduino_code/Test/src/mqtt_handler.cpp b/arduino_code/Test/src/mqtt_handler.cpp index 1395e0f..9d183b1 100644 --- a/arduino_code/Test/src/mqtt_handler.cpp +++ b/arduino_code/Test/src/mqtt_handler.cpp @@ -1,6 +1,7 @@ #include "mqtt_handler.h" #include "config.h" #include "motor.h" +#include "servo_control.h" #include #include @@ -29,6 +30,11 @@ static int last_pub_driver_status = -1; static int last_pub_tmc_software_enable = -1; static uint8_t last_pub_current_scaling = 255; +// Сервопривод +static int last_pub_angle = -1; +static int last_pub_enabled = -1; +static int last_pub_attached = -1; + static void setup_wifi() { Serial.print("Connecting to WiFi"); WiFi.begin(WIFI_SSID, WIFI_PASS); @@ -57,6 +63,11 @@ static void reconnect() { client.subscribe("motor/control/tmc/enable"); client.subscribe("motor/control/tmc/stealthchop"); client.subscribe("motor/control/tmc/coolstep"); + + client.subscribe("servo/control/angle"); + client.subscribe("servo/control/pulse"); + client.subscribe("servo/control/enable"); + client.subscribe("servo/control/detach"); } else { vTaskDelay(pdMS_TO_TICKS(5000)); } @@ -101,6 +112,20 @@ static void callback(char* topic, byte* payload, unsigned int length) { } else if (strcmp(topic, "motor/control/tmc/coolstep") == 0) { tmcSetCoolStep(strcmp(msg, "on") == 0); + } + else if (strcmp(topic, "servo/control/angle") == 0) { + servoSetAngle(atoi(msg)); + } + else if (strcmp(topic, "servo/control/pulse") == 0) { + servoSetPulse(atoi(msg)); + } + else if (strcmp(topic, "servo/control/enable") == 0) { + servoEnable(strcmp(msg, "on") == 0); + } + else if (strcmp(topic, "servo/control/detach") == 0) { + if (strcmp(msg, "1") == 0 || strcmp(msg, "true") == 0) { + servoDetach(); + } } } @@ -202,19 +227,54 @@ static void publishTelemetry() { last_pub_tmc_software_enable = tse; } } + + // Публикация угла (только при изменении) + int current_angle = servoGetAngle(); + if (current_angle != last_pub_angle) + { + client.publish("servo/feedback/angle", String(current_angle).c_str()); + last_pub_angle = current_angle; + } + + // Публикация статуса (только при изменении) + int enabled_int = servoIsEnabled() ? 1 : 0; + if (enabled_int != last_pub_enabled) + { + client.publish("servo/feedback/status", enabled_int ? "on" : "off"); + last_pub_enabled = enabled_int; + } + + // Публикация статуса (только при изменении) + int attached_int = servoIsAttached() ? 1 : 0; + if (attached_int != last_pub_attached) + { + client.publish("servo/feedback/status", attached_int ? "on" : "off"); + last_pub_attached = attached_int; + } + last_feedback_time = now; } } void mqttTask(void *parameter) { + static unsigned long last_stack_check = 0; setup_wifi(); client.setServer(MQTT_SERVER, MQTT_PORT); client.setCallback(callback); + servoInit(); + for (;;) { if (!client.connected()) reconnect(); client.loop(); publishTelemetry(); + + // Вывод свободного стека каждые 10 секунд + if (millis() - last_stack_check > 10000) { + Serial.printf("[STACK] Free: %u bytes\n", uxTaskGetStackHighWaterMark(NULL)); + last_stack_check = millis(); + } + vTaskDelay(pdMS_TO_TICKS(10)); } } \ No newline at end of file diff --git a/arduino_code/Test/src/servo_control.cpp b/arduino_code/Test/src/servo_control.cpp new file mode 100644 index 0000000..24977dc --- /dev/null +++ b/arduino_code/Test/src/servo_control.cpp @@ -0,0 +1,93 @@ +#include "servo_control.h" + +static Servo servo; +static int current_angle = 90; // Стартовое положение — центр +static bool servo_enabled = false; +static bool servo_attached = false; + +// ====================================================== +// Инициализация +// ====================================================== +void servoInit() { + // Разрешаем таймеры ESP32 для работы с серво + // ESP32PWM::allocateTimer(0); + // ESP32PWM::allocateTimer(1); + + servo.setPeriodHertz(50); // Стандартные 50 Гц для серво + servo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE); + servo_attached = true; + + // Устанавливаем стартовое положение + servo.write(current_angle); + servo_enabled = true; + + Serial.printf("[SERVO] Initialized on pin %d, angle=%d°\n", SERVO_PIN, current_angle); +} + +// ====================================================== +// Управление +// ====================================================== +void servoSetAngle(int angle) { + // Ограничиваем диапазон 0..180 + if (angle < 0) angle = 0; + if (angle > 180) angle = 180; + + current_angle = angle; + + if (servo_attached && servo_enabled) { + servo.write(current_angle); + } + Serial.printf("[SERVO] Angle set to %d°\n", current_angle); +} + +void servoSetPulse(uint16_t pulse_us) { + if (!servo_attached) return; + + // Ограничиваем диапазон импульса + if (pulse_us < SERVO_MIN_PULSE) pulse_us = SERVO_MIN_PULSE; + if (pulse_us > SERVO_MAX_PULSE) pulse_us = SERVO_MAX_PULSE; + + servo.writeMicroseconds(pulse_us); + + // Пересчитываем угол для телеметрии (линейная аппроксимация) + current_angle = map(pulse_us, SERVO_MIN_PULSE, SERVO_MAX_PULSE, 0, 180); + Serial.printf("[SERVO] Pulse set to %d µs (~%d°)\n", pulse_us, current_angle); +} + +void servoEnable(bool enable) { + servo_enabled = enable; + + if (servo_attached) { + if (servo_enabled) { + servo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE); + servo.write(current_angle); + Serial.println("[SERVO] Enabled"); + } else { + servo.detach(); + Serial.println("[SERVO] Disabled (detached)"); + } + } +} + +void servoDetach() { + if (servo_attached) { + servo.detach(); + servo_attached = false; + Serial.println("[SERVO] Detached"); + } +} + +// ====================================================== +// Получение состояния +// ====================================================== +int servoGetAngle() { + return current_angle; +} + +bool servoIsEnabled() { + return servo_enabled; +} + +bool servoIsAttached() { + return servo_attached; +} \ No newline at end of file diff --git a/arduino_code/Test/src/servo_control.h b/arduino_code/Test/src/servo_control.h new file mode 100644 index 0000000..0640ca4 --- /dev/null +++ b/arduino_code/Test/src/servo_control.h @@ -0,0 +1,21 @@ +#include +#include + +// --- Конфигурация --- +#define SERVO_PIN 19 +#define SERVO_MIN_PULSE 600 // мкс (соответствует 0°) +#define SERVO_MAX_PULSE 2400 // мкс (соответствует 180°) + +// --- Инициализация --- +void servoInit(); + +// --- Управление --- +void servoSetAngle(int angle); // 0..180 +void servoSetPulse(uint16_t pulse_us); // 500..2500 мкс +void servoEnable(bool enable); +void servoDetach(); + +// --- Получение состояния --- +int servoGetAngle(); +bool servoIsEnabled(); +bool servoIsAttached(); \ No newline at end of file