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