New: adding control servo-drive
This commit is contained in:
@@ -1,21 +1,24 @@
|
|||||||
# Документация MQTT-интерфейса управления шаговым двигателем (TMC2209)
|
# Документация MQTT-интерфейса управления двигателями (TMC2209 + Сервопривод)
|
||||||
|
|
||||||
Данный документ описывает структуру MQTT-топиков для удаленного управления шаговым двигателем и получения телеметрии с драйвера TMC2209.
|
Данный документ описывает структуру MQTT-топиков для удаленного управления шаговым двигателем с драйвером TMC2209 и сервоприводом, а также получения телеметрии с обоих устройств.
|
||||||
|
|
||||||
**Особенности реализации:**
|
**Общие особенности реализации:**
|
||||||
* **Интервал опроса:** Чтение и публикация статусов происходят каждые **500 мс**.
|
* **Интервал опроса:** Чтение и публикация статусов происходят каждые **500 мс**.
|
||||||
* **State-Change Publishing:** Для экономии трафика и снижения нагрузки на брокер, устройство публикует данные в топики телеметрии **только при изменении значения**.
|
* **State-Change Publishing:** Для экономии трафика и снижения нагрузки на брокер, устройство публикует данные в топики телеметрии **только при изменении значения**.
|
||||||
* **Сброс кэша:** При успешном переподключении к MQTT-брокеру внутренний кэш значений сбрасывается, что гарантирует отправку актуального состояния всех параметров сразу после установки связи.
|
* **Сброс кэша:** При успешном переподключении к MQTT-брокеру внутренний кэш значений сбрасывается, что гарантирует отправку актуального состояния всех параметров сразу после установки связи.
|
||||||
|
* **Единый контроллер:** Оба устройства управляются через один ESP32 с общей MQTT-инфраструктурой.
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 1. Топики управления (Subscribe)
|
## 1. Топики управления (Subscribe)
|
||||||
Устройство подписано на эти топики для получения команд от пользователя или вышестоящей системы (Home Assistant, Node-RED и т.д.).
|
Устройство подписано на эти топики для получения команд от пользователя или вышестоящей системы (Home Assistant, Node-RED и т.д.).
|
||||||
|
|
||||||
|
### 1.1. Управление шаговым двигателем
|
||||||
|
|
||||||
| Топик | Тип данных | Описание | Пример Payload |
|
| Топик | Тип данных | Описание | Пример Payload |
|
||||||
| :--- | :--- | :--- | :--- |
|
| :--- | :--- | :--- | :--- |
|
||||||
| `motor/control/rpm` | `int` | Установка целевой скорости вращения (об/мин). Отрицательные значения включают **реверс**. | `150` (вперед)<br>`-50` (назад) |
|
| `motor/control/rpm` | `int` | Установка целевой скорости вращения (об/мин). Отрицательные значения включают **реверс**. | `150` (вперед)<br>`-50` (назад) |
|
||||||
| `motor/control/driver` | `string` | Аппаратное управление пином `EN` драйвера. | `on` (включить)<br>*Любое другое* (выключить) |
|
| `motor/control/driver` | `string` | Аппаратное управление пином `EN` драйвера. | `on` (включить)<br>`off` (выключить) |
|
||||||
| `motor/control/totalsteps/reset` | `any` | Сброс счетчика шагов в ноль. После сброса устройство опубликует `0` в `motor/feedback/totalsteps`. | `1` / `reset` |
|
| `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/current_percent` | `int (0-100)` | Установка тока двигателя (Run Current) в процентах. Ток удержания (Hold) автоматически устанавливается как 50% от заданного. | `80` |
|
||||||
| `motor/control/tmc/microsteps` | `int` | Настройка режима микрошага. | `256`, `64`, `16`, `4` |
|
| `motor/control/tmc/microsteps` | `int` | Настройка режима микрошага. | `256`, `64`, `16`, `4` |
|
||||||
@@ -24,12 +27,21 @@
|
|||||||
| `motor/control/tmc/stealthchop` | `string` | Включение/выключение режима StealthChop (бесшумная работа на малых скоростях). | `on` / `off` |
|
| `motor/control/tmc/stealthchop` | `string` | Включение/выключение режима StealthChop (бесшумная работа на малых скоростях). | `on` / `off` |
|
||||||
| `motor/control/tmc/coolstep` | `string` | Включение/выключение режима CoolStep (адаптивное снижение тока для уменьшения нагрева). | `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` |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 2. Топики телеметрии (Publish)
|
## 2. Топики телеметрии (Publish)
|
||||||
Устройство публикует в эти топики текущее состояние двигателя и драйвера.
|
Устройство публикует в эти топики текущее состояние двигателей и драйверов.
|
||||||
|
|
||||||
### 2.1. Базовая телеметрия двигателя
|
### 2.1. Базовая телеметрия шагового двигателя
|
||||||
| Топик | Тип данных | Описание | Пример Payload |
|
| Топик | Тип данных | Описание | Пример Payload |
|
||||||
| :--- | :--- | :--- | :--- |
|
| :--- | :--- | :--- | :--- |
|
||||||
| `motor/feedback/rpm` | `int` | Текущая фактическая скорость вращения (об/мин). Может быть отрицательной при реверсе. | `145` |
|
| `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/stealth_chop_active` | `bool (string)` | Находится ли драйвер в режиме StealthChop в данный момент. | `true` / `false` |
|
||||||
| `motor/feedback/tmc/status/standstill` | `bool (string)` | Находится ли двигатель в состоянии покоя (Standstill). | `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/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 |
|
| Топик | Тип данных | Описание | Пример Payload |
|
||||||
| :--- | :--- | :--- | :--- |
|
| :--- | :--- | :--- | :--- |
|
||||||
| `motor/feedback/driver/status` | `string` | Общий результат аппаратной проверки драйвера (`checkDriverStatus()`). | `on` / `off` |
|
| `servo/feedback/angle` | `int` | Текущий установленный угол поворота (0-180). | `90` |
|
||||||
| `motor/feedback/tmc/status` | `string` | Результат проверки программного флага включения TMC (`checkTmcSoftwareEnable()`). | `on` / `off` |
|
| `servo/feedback/status` | `string` | Статус сервопривода (включен/выключен). | `on` / `off` |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 3. Примеры сценариев использования
|
## 3. Примеры сценариев использования
|
||||||
|
|
||||||
### Запуск двигателя с нужными параметрами
|
### 3.1. Управление шаговым двигателем
|
||||||
|
|
||||||
|
#### Запуск двигателя с нужными параметрами
|
||||||
```bash
|
```bash
|
||||||
# Устанавливаем ток 70% и микрошаг 16
|
# Устанавливаем ток 70% и микрошаг 16
|
||||||
mosquitto_pub -t "motor/control/tmc/current_percent" -m "70"
|
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"
|
mosquitto_pub -t "motor/control/tmc/stealthchop" -m "on"
|
||||||
|
|
||||||
# Аппаратно включаем драйвер и задаем скорость 100 RPM
|
# Аппаратно включаем драйвер и задаем скорость 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"
|
mosquitto_pub -t "motor/control/rpm" -m "100"
|
||||||
```
|
```
|
||||||
|
|
||||||
### Аварийная остановка и сброс
|
#### Аварийная остановка и сброс
|
||||||
```bash
|
```bash
|
||||||
# Остановка (скорость 0)
|
# Остановка (скорость 0)
|
||||||
mosquitto_pub -t "motor/control/rpm" -m "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"
|
mosquitto_pub -t "motor/control/totalsteps/reset" -m "1"
|
||||||
```
|
```
|
||||||
|
|
||||||
### Мониторинг нагрузки (StallGuard)
|
#### Мониторинг нагрузки (StallGuard)
|
||||||
Для отслеживания нагрузки на вал или момента срыва шагов, необходимо слушать топик `motor/feedback/tmc/sg_result`. Резкое падение значения `sg_result` при движении обычно означает столкновение или заклинивание механизма.
|
Для отслеживания нагрузки на вал или момента срыва шагов, необходимо слушать топик `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/#"
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
|||||||
@@ -18,3 +18,4 @@ upload_port = /dev/ttyUSB0
|
|||||||
lib_deps =
|
lib_deps =
|
||||||
knolleary/PubSubClient@^2.8
|
knolleary/PubSubClient@^2.8
|
||||||
janelia-arduino/TMC2209@^9.4.0
|
janelia-arduino/TMC2209@^9.4.0
|
||||||
|
madhephaestus/ESP32Servo@^3.2.1
|
||||||
|
|||||||
@@ -14,7 +14,7 @@ void setup() {
|
|||||||
xTaskCreatePinnedToCore(
|
xTaskCreatePinnedToCore(
|
||||||
mqttTask,
|
mqttTask,
|
||||||
"MQTT_Task",
|
"MQTT_Task",
|
||||||
8192,
|
20480,
|
||||||
NULL,
|
NULL,
|
||||||
3,
|
3,
|
||||||
NULL,
|
NULL,
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
#include "mqtt_handler.h"
|
#include "mqtt_handler.h"
|
||||||
#include "config.h"
|
#include "config.h"
|
||||||
#include "motor.h"
|
#include "motor.h"
|
||||||
|
#include "servo_control.h"
|
||||||
#include <WiFi.h>
|
#include <WiFi.h>
|
||||||
#include <PubSubClient.h>
|
#include <PubSubClient.h>
|
||||||
|
|
||||||
@@ -29,6 +30,11 @@ static int last_pub_driver_status = -1;
|
|||||||
static int last_pub_tmc_software_enable = -1;
|
static int last_pub_tmc_software_enable = -1;
|
||||||
static uint8_t last_pub_current_scaling = 255;
|
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() {
|
static void setup_wifi() {
|
||||||
Serial.print("Connecting to WiFi");
|
Serial.print("Connecting to WiFi");
|
||||||
WiFi.begin(WIFI_SSID, WIFI_PASS);
|
WiFi.begin(WIFI_SSID, WIFI_PASS);
|
||||||
@@ -57,6 +63,11 @@ static void reconnect() {
|
|||||||
client.subscribe("motor/control/tmc/enable");
|
client.subscribe("motor/control/tmc/enable");
|
||||||
client.subscribe("motor/control/tmc/stealthchop");
|
client.subscribe("motor/control/tmc/stealthchop");
|
||||||
client.subscribe("motor/control/tmc/coolstep");
|
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 {
|
} else {
|
||||||
vTaskDelay(pdMS_TO_TICKS(5000));
|
vTaskDelay(pdMS_TO_TICKS(5000));
|
||||||
}
|
}
|
||||||
@@ -102,6 +113,20 @@ static void callback(char* topic, byte* payload, unsigned int length) {
|
|||||||
else if (strcmp(topic, "motor/control/tmc/coolstep") == 0) {
|
else if (strcmp(topic, "motor/control/tmc/coolstep") == 0) {
|
||||||
tmcSetCoolStep(strcmp(msg, "on") == 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();
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void publishTelemetry() {
|
static void publishTelemetry() {
|
||||||
@@ -202,19 +227,54 @@ static void publishTelemetry() {
|
|||||||
last_pub_tmc_software_enable = tse;
|
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;
|
last_feedback_time = now;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void mqttTask(void *parameter) {
|
void mqttTask(void *parameter) {
|
||||||
|
static unsigned long last_stack_check = 0;
|
||||||
setup_wifi();
|
setup_wifi();
|
||||||
client.setServer(MQTT_SERVER, MQTT_PORT);
|
client.setServer(MQTT_SERVER, MQTT_PORT);
|
||||||
client.setCallback(callback);
|
client.setCallback(callback);
|
||||||
|
|
||||||
|
servoInit();
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
if (!client.connected()) reconnect();
|
if (!client.connected()) reconnect();
|
||||||
client.loop();
|
client.loop();
|
||||||
publishTelemetry();
|
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));
|
vTaskDelay(pdMS_TO_TICKS(10));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
93
arduino_code/Test/src/servo_control.cpp
Normal file
93
arduino_code/Test/src/servo_control.cpp
Normal file
@@ -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;
|
||||||
|
}
|
||||||
21
arduino_code/Test/src/servo_control.h
Normal file
21
arduino_code/Test/src/servo_control.h
Normal file
@@ -0,0 +1,21 @@
|
|||||||
|
#include <Arduino.h>
|
||||||
|
#include <ESP32Servo.h>
|
||||||
|
|
||||||
|
// --- Конфигурация ---
|
||||||
|
#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();
|
||||||
Reference in New Issue
Block a user