Update: replace board, add servo driver program
This commit is contained in:
@@ -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` (вперед)<br>`-50` (назад) |
|
||||
| `motor/control/driver` | `string` | Аппаратное управление пином `EN` драйвера. | `on` (включить)<br>`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` (центр)<br>`0` (мин)<br>`180` (макс) |
|
||||
| `servo/control/pulse` | `int (500-2500)` | Прямая установка ширины управляющего импульса в микросекундах. | `1500` (центр)<br>`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`).
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
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
|
||||
@@ -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
|
||||
@@ -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();
|
||||
|
||||
@@ -4,5 +4,6 @@
|
||||
#include <Arduino.h>
|
||||
|
||||
void mqttTask(void *parameter);
|
||||
void handleServoMQTTCommand(const char* topic, const char* payload);
|
||||
|
||||
#endif
|
||||
@@ -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];
|
||||
}
|
||||
@@ -1,21 +1,32 @@
|
||||
#ifndef SERVO_H
|
||||
#define SERVO_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <ESP32Servo.h>
|
||||
#include <Adafruit_PWMServoDriver.h>
|
||||
|
||||
// --- Конфигурация ---
|
||||
#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();
|
||||
// Получение состояния
|
||||
uint8_t getServoAngle(uint8_t channel);
|
||||
bool isServoEnabled(uint8_t channel);
|
||||
|
||||
#endif // SERVO_H
|
||||
Reference in New Issue
Block a user