From ff51eccf221b0fbab65d9287de33fd422a8fa896 Mon Sep 17 00:00:00 2001 From: drho1y Date: Wed, 15 Jul 2026 23:44:36 +0300 Subject: [PATCH] Update: replace board, add servo driver program --- arduino_code/Test/README.md | 265 ++++++++++-------------- arduino_code/Test/platformio.ini | 1 + arduino_code/Test/src/config.cpp | 18 +- arduino_code/Test/src/config.h | 6 +- arduino_code/Test/src/mqtt_handler.cpp | 120 +++++++---- arduino_code/Test/src/mqtt_handler.h | 1 + arduino_code/Test/src/servo_control.cpp | 141 +++++++------ arduino_code/Test/src/servo_control.h | 41 ++-- 8 files changed, 307 insertions(+), 286 deletions(-) diff --git a/arduino_code/Test/README.md b/arduino_code/Test/README.md index ebc744a..e16c65c 100644 --- a/arduino_code/Test/README.md +++ b/arduino_code/Test/README.md @@ -1,176 +1,129 @@ -# Документация MQTT-интерфейса управления двигателями (TMC2209 + Сервопривод) - -Данный документ описывает структуру MQTT-топиков для удаленного управления шаговым двигателем с драйвером TMC2209 и сервоприводом, а также получения телеметрии с обоих устройств. - -**Общие особенности реализации:** -* **Интервал опроса:** Чтение и публикация статусов происходят каждые **500 мс**. -* **State-Change Publishing:** Для экономии трафика и снижения нагрузки на брокер, устройство публикует данные в топики телеметрии **только при изменении значения**. -* **Сброс кэша:** При успешном переподключении к MQTT-брокеру внутренний кэш значений сбрасывается, что гарантирует отправку актуального состояния всех параметров сразу после установки связи. -* **Единый контроллер:** Оба устройства управляются через один ESP32 с общей MQTT-инфраструктурой. +Ниже представлена структурированная документация по MQTT-топикам, составленная на основе предоставленного вами кода. Документация разделена на логические блоки для удобства использования (управление и телеметрия). --- -## 1. Топики управления (Subscribe) -Устройство подписано на эти топики для получения команд от пользователя или вышестоящей системы (Home Assistant, Node-RED и т.д.). +# 📡 Документация по MQTT интерфейсу (ESP32 + TMC2209 + Servo) -### 1.1. Управление шаговым двигателем - -| Топик | Тип данных | Описание | Пример Payload | -| :--- | :--- | :--- | :--- | -| `motor/control/rpm` | `int` | Установка целевой скорости вращения (об/мин). Отрицательные значения включают **реверс**. | `150` (вперед)
`-50` (назад) | -| `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` | -| `motor/control/tmc/stallguard` | `int` | Установка порогового значения StallGuard (защита от пропуска шагов / датчик нагрузки). | `5` | -| `motor/control/tmc/enable` | `string` | Программное включение/выключение чипа TMC2209. | `on` / `off` | -| `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` | +## 📌 Общая информация +- **Архитектура**: ESP32 (FreeRTOS задача `mqttTask`). +- **Период опроса телеметрии**: 500 мс. +- **Оптимизация трафика**: Публикация данных происходит **только при изменении значения** (реализован кэш последних отправленных значений). +- **Префиксы**: + - `.../control/...` — топики для **отправки команд** устройству (подписка устройства). + - `.../feedback/...` — топики для **получения статуса/телеметрии** от устройства (публикация устройства). --- -## 2. Топики телеметрии (Publish) -Устройство публикует в эти топики текущее состояние двигателей и драйверов. +## ⚙️ 1. Управление шаговым двигателем (Motor Control) -### 2.1. Базовая телеметрия шагового двигателя -| Топик | Тип данных | Описание | Пример Payload | -| :--- | :--- | :--- | :--- | -| `motor/feedback/rpm` | `int` | Текущая фактическая скорость вращения (об/мин). Может быть отрицательной при реверсе. | `145` | -| `motor/feedback/totalsteps` | `unsigned long` | Накопительный счетчик шагов. | `145000` | -| `motor/feedback/is_run` | `bool (string)` | Флаг вращения двигателя. | `true` / `false` | +Устройство подписывается на эти топики для получения команд. -### 2.2. Телеметрия настроек TMC2209 -| Топик | Тип данных | Описание | Пример Payload | -| :--- | :--- | :--- | :--- | -| `motor/feedback/tmc/current_percent` | `int` | Текущий установленный рабочий ток (Run Current) в %. | `80` | -| `motor/feedback/tmc/microsteps` | `int` | Текущее значение делителя микрошага. | `16` | -| `motor/feedback/tmc/sg_result` | `uint16_t` | Текущее считанное значение StallGuard (индикатор нагрузки на вал). | `12` | -| `motor/feedback/tmc/interstep_duration` | `uint32_t` | Текущая длительность интервала между шагами (в тиках/микросекундах). | `4500` | - -### 2.3. Статусы и флаги драйвера (DRV_STATUS) -Эти топики отражают аппаратные флаги, считываемые из регистра статуса TMC2209. - -| Топик | Тип данных | Описание | Пример Payload | -| :--- | :--- | :--- | :--- | -| `motor/feedback/tmc/status/over_temp` | `bool (string)` | Перегрев драйвера (предупреждение или аварийное отключение). | `true` / `false` | -| `motor/feedback/tmc/status/short_to_ground` | `bool (string)` | Короткое замыкание на землю (на фазах A или B). | `true` / `false` | -| `motor/feedback/tmc/status/open_load` | `bool (string)` | Обрыв нагрузки (не подключена катушка двигателя). | `true` / `false` | -| `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. Телеметрия сервопривода -| Топик | Тип данных | Описание | Пример Payload | -| :--- | :--- | :--- | :--- | -| `servo/feedback/angle` | `int` | Текущий установленный угол поворота (0-180). | `90` | -| `servo/feedback/status` | `string` | Статус сервопривода (включен/выключен). | `on` / `off` | +| Топик | Тип данных | Описание | Пример полезной нагрузки (Payload) | +| :--- | :---: | :--- | :--- | +| `motor/control/rpm` | Integer | Целевая скорость в об/мин (RPM). **Отрицательные значения** включают реверс. | `-150`, `0`, `300` | +| `motor/control/driver` | String | Аппаратное включение/выключение драйвера (пин `EN_PIN`). `on` = LOW (включен), любое другое = HIGH (выключен). | `on`, `off` | +| `motor/control/totalsteps/reset` | Any | Сброс счетчика шагов в ноль. После сброса устройство само опубликует `0` в топик обратной связи. | `1`, `reset`, `""` | --- -## 3. Примеры сценариев использования +## 🛠️ 2. Управление драйвером TMC2209 (TMC Control) -### 3.1. Управление шаговым двигателем +Специфические команды для настройки микросхемы TMC2209. + +| Топик | Тип данных | Описание | Пример полезной нагрузки | +| :--- | :---: | :--- | :--- | +| `motor/control/tmc/current_percent` | Integer (0-100) | Установка рабочего тока в процентах. Ток удержания (Hold) автоматически устанавливается как 50% от рабочего. | `75` | +| `motor/control/tmc/microsteps` | Integer | Установка режима микрошага. | `16`, `32`, `256` | +| `motor/control/tmc/stallguard` | Integer | Установка порога чувствительности StallGuard (защита от заклинивания/определение нагрузки). | `10`, `50` | +| `motor/control/tmc/enable` | String | Программное включение/выключение чипа TMC. | `on`, `off` | +| `motor/control/tmc/stealthchop` | String | Включение/выключение режима StealthChop (бесшумный режим). | `on`, `off` | +| `motor/control/tmc/coolstep` | String | Включение/выключение режима CoolStep (динамическое управление током). | `on`, `off` | + +--- + +## 📊 3. Телеметрия шагового двигателя (Motor Feedback) + +Устройство публикует эти данные **только при изменении значения**, проверка происходит каждые 500 мс. + +| Топик | Тип данных | Описание | +| :--- | :---: | :--- | +| `motor/feedback/rpm` | Integer | Текущая фактическая скорость вращения (RPM). | +| `motor/feedback/totalsteps` | Integer (String) | Общее количество сделанных шагов (накапливаемое). | +| `motor/feedback/is_run` | String (`true`/`false`) | Флаг: двигатель в движении (`true`) или остановлен (`false`). | +| `motor/feedback/driver/status` | String (`on`/`off`) | Общий статус драйвера (результат `checkDriverStatus()`). | +| `motor/feedback/tmc/status` | String (`on`/`off`) | Статус программного включения TMC (результат `checkTmcSoftwareEnable()`). | + +### Детальная телеметрия TMC2209 +*(Публикуется только если `tmcIsInitialized() == true`)* + +| Топик | Тип данных | Описание | +| :--- | :---: | :--- | +| `motor/feedback/tmc/current_percent` | Integer | Текущий установленный процент рабочего тока. | +| `motor/feedback/tmc/microsteps` | Integer | Текущее значение микрошага. | +| `motor/feedback/tmc/sg_result` | Integer | Текущее значение StallGuard (нагрузка на вал). | +| `motor/feedback/tmc/interstep_duration`| Integer | Длительность между шагами (мкс/такты). | +| `motor/feedback/tmc/status/over_temp` | String (`true`/`false`) | Предупреждение или отключение по перегреву. | +| `motor/feedback/tmc/status/short_to_ground`| String (`true`/`false`) | Короткое замыкание на землю (фаза A или B). | +| `motor/feedback/tmc/status/open_load` | String (`true`/`false`) | Обрыв нагрузки (отключен двигатель) на фазе A или B. | +| `motor/feedback/tmc/status/stealth_chop_active`| String (`true`/`false`) | Фактически активный режим StealthChop. | +| `motor/feedback/tmc/status/standstill` | String (`true`/`false`) | Двигатель находится в состоянии покоя. | +| `motor/feedback/tmc/status/current_scaling`| Integer | Внутренний масштабный коэффициент тока драйвера. | + +--- + +## 🦾 4. Управление сервоприводами (Servo Control) + +Поддержка нескольких каналов (сервоприводов). Топики используют динамический параметр `{channel}` (номер канала, от `0` до `MAX_SERVOS - 1`). + +### Команды (Подписка устройства) +Базовый паттерн: `servo/control/{channel}/{command}` + +| Топик (пример для канала 0) | Тип данных | Описание | Пример Payload | +| :--- | :---: | :--- | :--- | +| `servo/control/0/angle` | Integer (0-180) | Установить угол поворота сервопривода. | `90`, `180`, `0` | +| `servo/control/0/enable` | String | Включить (`on`, `1`, `true`) или выключить сервопривод. | `on`, `off`, `true` | + +*Примечание: Код использует `sscanf(topic, "servo/control/%d/%s", &channel, command)`, поэтому команда может быть любой строкой, но обрабатываются только `angle` и `enable`.* + +### Обратная связь (Публикация устройства) +Устройство публикует состояние конкретного канала только при его изменении. + +| Топик (пример для канала 0) | Тип данных | Описание | +| :--- | :---: | :--- | +| `servo/0/feedback/status` | String (`on`/`off`) | Текущий статус включения сервопривода на канале 0. | +| `servo/0/feedback/angle` | Integer | Текущий установленный угол (в градусах) сервопривода на канале 0. | + +*(Замените `0` на актуальный номер канала при использовании)* + +--- + +## 💡 Важные особенности реализации + +1. **Анти-спам (Кэширование)**: В коде реализована строгая проверка `if (current_value != last_pub_value)`. Это означает, что если двигатель стоит, а его параметры не меняются, топик `motor/feedback/rpm` **не будет** засорять сеть сообщениями каждые 500 мс. Сообщение придет только при изменении. +2. **Сброс шагов**: При получении команды на `motor/control/totalsteps/reset`, устройство не только сбрасывает внутренний счетчик, но и принудительно публикует `"0"` в `motor/feedback/totalsteps`, чтобы синхронизировать состояние с клиентом. +3. **Логика пина Enable**: Для `motor/control/driver` значение `"on"` подает `LOW` на `EN_PIN` (что стандартно для TMC2209 означает **включение** драйвера), а любое другое значение подает `HIGH` (выключение). +4. **Безопасность серво**: При отправке команды `angle`, код проверяет диапазон `0 <= angle <= 180`. Значения вне этого диапазона будут проигнорированы. + +--- + +### Пример сценария использования (CLI / mosquitto) -#### Запуск двигателя с нужными параметрами ```bash -# Устанавливаем ток 70% и микрошаг 16 -mosquitto_pub -t "motor/control/tmc/current_percent" -m "70" -mosquitto_pub -t "motor/control/tmc/microsteps" -m "16" +# Включить драйвер +mosquitto_pub -t "motor/control/driver" -m "on" -# Включаем тихий режим +# Запустить двигатель на 200 об/мин вперед +mosquitto_pub -t "motor/control/rpm" -m "200" + +# Включить бесшумный режим mosquitto_pub -t "motor/control/tmc/stealthchop" -m "on" -# Аппаратно включаем драйвер и задаем скорость 100 RPM -mosquitto_pub -t "motor/control/driver" -m "on" -mosquitto_pub -t "motor/control/rpm" -m "100" +# Повернуть сервопривод на канале 1 на 90 градусов и включить его +mosquitto_pub -t "servo/control/1/enable" -m "on" +mosquitto_pub -t "servo/control/1/angle" -m "90" + +# Подписаться на всю телеметрию двигателя для отладки +mosquitto_sub -t "motor/feedback/#" -v ``` -#### Аварийная остановка и сброс -```bash -# Остановка (скорость 0) -mosquitto_pub -t "motor/control/rpm" -m "0" - -# Программное отключение чипа TMC -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` при движении обычно означает столкновение или заклинивание механизма. - -### 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/#" -``` - ---- - +Если вам нужно добавить эту документацию в ваш репозиторий, я могу оформить её в виде готового файла `README.md` или `MQTT_API.md` с дополнительными разделами (например, схемой подключения или настройкой `config.h`). \ No newline at end of file diff --git a/arduino_code/Test/platformio.ini b/arduino_code/Test/platformio.ini index 01ee9e0..1464849 100644 --- a/arduino_code/Test/platformio.ini +++ b/arduino_code/Test/platformio.ini @@ -19,3 +19,4 @@ lib_deps = knolleary/PubSubClient@^2.8 janelia-arduino/TMC2209@^9.4.0 madhephaestus/ESP32Servo@^3.2.1 + adafruit/Adafruit PWM Servo Driver Library@^3.0.3 diff --git a/arduino_code/Test/src/config.cpp b/arduino_code/Test/src/config.cpp index 7a97b10..23f78b8 100644 --- a/arduino_code/Test/src/config.cpp +++ b/arduino_code/Test/src/config.cpp @@ -1,8 +1,8 @@ #include "config.h" -const char* WIFI_SSID = "Home"; -const char* WIFI_PASS = "88888888qwE"; -const char* MQTT_SERVER = "192.168.31.225"; +const char* WIFI_SSID = "TP-Link_3E5C"; +const char* WIFI_PASS = "12697571"; +const char* MQTT_SERVER = "192.168.0.200"; const int MQTT_PORT = 1883; const char* MQTT_USER = "test"; const char* MQTT_PASS = "1234"; @@ -25,4 +25,14 @@ const unsigned long RAMP_DURATION_MS = 2000; 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 +const uint16_t TMC_MICROSTEPS = 1; + +// I2C Настройка +const int16_t I2C_SDA_PIN = 21; +const int16_t I2C_SLC_PIN = 22; + + +// ========== SERVO CONFIGURATION ========== +#define SERVO_DEFAULT_CHANNEL 0 +#define SERVO_MIN_ANGLE 0 +#define SERVO_MAX_ANGLE 180 \ No newline at end of file diff --git a/arduino_code/Test/src/config.h b/arduino_code/Test/src/config.h index 6789698..5046fe2 100644 --- a/arduino_code/Test/src/config.h +++ b/arduino_code/Test/src/config.h @@ -29,6 +29,10 @@ extern const unsigned long RAMP_DURATION_MS; 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; +extern const uint16_t TMC_MICROSTEPS; + +// I2C Настройка +extern const int16_t I2C_SDA_PIN; +extern const int16_t I2C_SLC_PIN; #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 9d183b1..f77a4d2 100644 --- a/arduino_code/Test/src/mqtt_handler.cpp +++ b/arduino_code/Test/src/mqtt_handler.cpp @@ -64,10 +64,7 @@ static void reconnect() { 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"); + client.subscribe("servo/control/+/#"); } else { vTaskDelay(pdMS_TO_TICKS(5000)); } @@ -112,20 +109,8 @@ 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(); - } + } if (strncmp(topic, "servo/control", 12) == 0) { + handleServoMQTTCommand(topic, msg); } } @@ -228,34 +213,85 @@ static void publishTelemetry() { } } - // Публикация угла (только при изменении) - 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 checkAndPublishServoStatus(uint8_t channel) { + if (channel >= MAX_SERVOS) return; + + bool current_state = servo_enabled[channel]; + if (current_state != last_published_status[channel]) { + last_published_status[channel] = current_state; + + char topic[64]; + snprintf(topic, sizeof(topic), "servo/%d/feedback/status", channel); + + String status = current_state ? "on" : "off"; + client.publish(topic, status.c_str()); + + Serial.printf("Published servo %d status: %s\n", channel, status.c_str()); + } +} + +void checkAndPublishServoAngle(uint8_t channel) { + if (channel >= MAX_SERVOS) return; + + uint8_t current_angle = current_angles[channel]; + if (current_angle != last_published_angles[channel]) { + last_published_angles[channel] = current_angle; + + char topic[64]; + snprintf(topic, sizeof(topic), "servo/%d/feedback/angle", channel); + + char payload[8]; + snprintf(payload, sizeof(payload), "%d", current_angle); + client.publish(topic, payload); + + Serial.printf("Published servo %d angle: %d\n", channel, current_angle); + } +} + +void handleServoMQTTCommand(const char* topic, const char* payload) { + // Парсим топик: servo/control/{channel}/{command} + int channel = -1; + char command[32] = {0}; + + if (sscanf(topic, "servo/control/%d/%s", &channel, command) != 2) { + Serial.printf("Invalid servo topic: %s\n", topic); + return; + } + + if (channel < 0 || channel >= MAX_SERVOS) { + Serial.printf("Invalid servo channel: %d\n", channel); + return; + } + + Serial.printf("Servo %d command: %s = %s\n", channel, command, payload); + + if (strcmp(command, "angle") == 0) { + int angle = atoi(payload); + if (angle >= 0 && angle <= 180) { + setServoAngle(channel, (uint8_t)angle); + checkAndPublishServoAngle(channel); + checkAndPublishServoStatus(channel); + } + } + else if (strcmp(command, "enable") == 0) { + bool enable = (strcmp(payload, "on") == 0 || strcmp(payload, "1") == 0 || strcmp(payload, "true") == 0); + if (enable) { + enableServo(channel); + } else { + disableServo(channel); + } + checkAndPublishServoStatus(channel); + } +} + +///// + void mqttTask(void *parameter) { static unsigned long last_stack_check = 0; setup_wifi(); diff --git a/arduino_code/Test/src/mqtt_handler.h b/arduino_code/Test/src/mqtt_handler.h index 075c909..0ea14d9 100644 --- a/arduino_code/Test/src/mqtt_handler.h +++ b/arduino_code/Test/src/mqtt_handler.h @@ -4,5 +4,6 @@ #include void mqttTask(void *parameter); +void handleServoMQTTCommand(const char* topic, const char* payload); #endif \ No newline at end of file diff --git a/arduino_code/Test/src/servo_control.cpp b/arduino_code/Test/src/servo_control.cpp index 24977dc..f14f3da 100644 --- a/arduino_code/Test/src/servo_control.cpp +++ b/arduino_code/Test/src/servo_control.cpp @@ -1,93 +1,98 @@ #include "servo_control.h" +#include "mqtt_handler.h" +#include "config.h" + +static Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); +static bool servo_initialized = false; + -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); + Serial.println("Initializing PCA9685 servo driver..."); - servo.setPeriodHertz(50); // Стандартные 50 Гц для серво - servo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE); - servo_attached = true; + // Инициализация I2C на пинах 21 (SDA) и 22 (SCL) + Wire.begin(I2C_SDA_PIN, I2C_SLC_PIN); - // Устанавливаем стартовое положение - servo.write(current_angle); - servo_enabled = true; + pwm.begin(); + pwm.setOscillatorFrequency(27000000); + pwm.setPWMFreq(50); // 50 Hz для сервоприводов - Serial.printf("[SERVO] Initialized on pin %d, angle=%d°\n", SERVO_PIN, current_angle); + delay(10); + + servo_initialized = true; + Serial.println("PCA9685 initialized successfully"); + + // Инициализируем все каналы как выключенные + for (int i = 0; i < MAX_SERVOS; i++) { + current_angles[i] = 90; // Начальное положение - середина + servo_enabled[i] = false; + disableServo(i); + } } -// ====================================================== -// Управление -// ====================================================== -void servoSetAngle(int angle) { - // Ограничиваем диапазон 0..180 - if (angle < 0) angle = 0; +// Преобразование угла (0-180) в длину импульса +uint16_t angleToPulse(uint8_t angle) { + if (angle > 180) angle = 180; + return map(angle, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE); +} + +// Преобразование длины импульса в угол +uint8_t pulseToAngle(uint16_t pulse) { + if (pulse < SERVO_MIN_PULSE) return 0; + if (pulse > SERVO_MAX_PULSE) return 180; + return map(pulse, SERVO_MIN_PULSE, SERVO_MAX_PULSE, 0, 180); +} + +void setServoAngle(uint8_t channel, uint8_t angle) { + if (!servo_initialized || channel >= MAX_SERVOS) return; + if (angle > 180) angle = 180; - current_angle = angle; + current_angles[channel] = angle; + servo_enabled[channel] = true; - 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; + uint16_t pulse = angleToPulse(angle); + setServoPulse(channel, pulse); - // Ограничиваем диапазон импульса - if (pulse_us < SERVO_MIN_PULSE) pulse_us = SERVO_MIN_PULSE; - if (pulse_us > SERVO_MAX_PULSE) pulse_us = SERVO_MAX_PULSE; + Serial.printf("Servo %d: angle=%d, pulse=%d\n", channel, angle, pulse); +} + +void setServoPulse(uint8_t channel, uint16_t pulse) { + if (!servo_initialized || channel >= MAX_SERVOS) return; - servo.writeMicroseconds(pulse_us); + // Преобразование микросекунд в тики PCA9685 + // PCA9685 имеет 4096 тиков на период при 50Hz = 20000 мкс + // 1 мкс = 4096 / 20000 = 0.2048 тика + double pulselength = 4096.0 / 20000.0; // тиков на микросекунду + uint16_t ticks = pulse * pulselength; - // Пересчитываем угол для телеметрии (линейная аппроксимация) - 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); + pwm.setPWM(channel, 0, ticks); } -void servoEnable(bool enable) { - servo_enabled = enable; +void enableServo(uint8_t channel) { + if (!servo_initialized || channel >= MAX_SERVOS) return; - 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)"); - } - } + servo_enabled[channel] = true; + setServoAngle(channel, current_angles[channel]); + + Serial.printf("Servo %d: ENABLED\n", channel); } -void servoDetach() { - if (servo_attached) { - servo.detach(); - servo_attached = false; - Serial.println("[SERVO] Detached"); - } +void disableServo(uint8_t channel) { + if (!servo_initialized || channel >= MAX_SERVOS) return; + + servo_enabled[channel] = false; + pwm.setPWM(channel, 0, 0); // Отключаем сигнал + + Serial.printf("Servo %d: DISABLED\n", channel); } -// ====================================================== -// Получение состояния -// ====================================================== -int servoGetAngle() { - return current_angle; +uint8_t getServoAngle(uint8_t channel) { + if (channel >= MAX_SERVOS) return 0; + return current_angles[channel]; } -bool servoIsEnabled() { - return servo_enabled; -} - -bool servoIsAttached() { - return servo_attached; +bool isServoEnabled(uint8_t channel) { + if (channel >= MAX_SERVOS) return false; + return servo_enabled[channel]; } \ No newline at end of file diff --git a/arduino_code/Test/src/servo_control.h b/arduino_code/Test/src/servo_control.h index 0640ca4..0f6b203 100644 --- a/arduino_code/Test/src/servo_control.h +++ b/arduino_code/Test/src/servo_control.h @@ -1,21 +1,32 @@ +#ifndef SERVO_H +#define SERVO_H + #include -#include +#include -// --- Конфигурация --- -#define SERVO_PIN 19 -#define SERVO_MIN_PULSE 600 // мкс (соответствует 0°) -#define SERVO_MAX_PULSE 2400 // мкс (соответствует 180°) -// --- Инициализация --- +// Хранение текущего состояния сервоприводов +#define MAX_SERVOS 16 +static uint8_t current_angles[MAX_SERVOS] = {0}; +static bool servo_enabled[MAX_SERVOS] = {false}; +static uint8_t last_published_angles[MAX_SERVOS] = {255}; +static bool last_published_status[MAX_SERVOS] = {false}; + +// Минимальная и максимальная длина импульса для сервопривода (в микросекундах) +static const uint16_t SERVO_MIN_PULSE = 600; +static const uint16_t SERVO_MAX_PULSE = 2400; + +// Инициализация сервопривода void servoInit(); -// --- Управление --- -void servoSetAngle(int angle); // 0..180 -void servoSetPulse(uint16_t pulse_us); // 500..2500 мкс -void servoEnable(bool enable); -void servoDetach(); +// Управление сервоприводом +void setServoAngle(uint8_t channel, uint8_t angle); +void setServoPulse(uint8_t channel, uint16_t pulse); +void enableServo(uint8_t channel); +void disableServo(uint8_t channel); -// --- Получение состояния --- -int servoGetAngle(); -bool servoIsEnabled(); -bool servoIsAttached(); \ No newline at end of file +// Получение состояния +uint8_t getServoAngle(uint8_t channel); +bool isServoEnabled(uint8_t channel); + +#endif // SERVO_H \ No newline at end of file