chore: add unique assets from other branches into dan_branch
Bring hardware/spec content from drho1y-mvp_1 (3d_models, arduino_code, backend_control, kicad, specification) so dan_branch holds the shared union of branch files without rewriting other branch tips. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Даня Архипов
5
arduino_code/Test/.gitignore
vendored
Normal file
5
arduino_code/Test/.gitignore
vendored
Normal file
@@ -0,0 +1,5 @@
|
||||
.pio
|
||||
.vscode/.browse.c_cpp.db*
|
||||
.vscode/c_cpp_properties.json
|
||||
.vscode/launch.json
|
||||
.vscode/ipch
|
||||
10
arduino_code/Test/.vscode/extensions.json
vendored
Normal file
10
arduino_code/Test/.vscode/extensions.json
vendored
Normal file
@@ -0,0 +1,10 @@
|
||||
{
|
||||
// See http://go.microsoft.com/fwlink/?LinkId=827846
|
||||
// for the documentation about the extensions.json format
|
||||
"recommendations": [
|
||||
"platformio.platformio-ide"
|
||||
],
|
||||
"unwantedRecommendations": [
|
||||
"ms-vscode.cpptools-extension-pack"
|
||||
]
|
||||
}
|
||||
138
arduino_code/Test/README.md
Normal file
138
arduino_code/Test/README.md
Normal file
@@ -0,0 +1,138 @@
|
||||
Вот обновленная и расширенная документация по MQTT-интерфейсу, включающая новую функциональность работы с датчиками расстояния **VL53L0X**, а также оптимизации, появившиеся в коде.
|
||||
|
||||
---
|
||||
|
||||
# 📡 Документация по MQTT интерфейсу (ESP32 + TMC2209 + Servo + VL53L0X)
|
||||
|
||||
## 📌 Общая информация
|
||||
- **Архитектура**: ESP32 (FreeRTOS задача `mqttTask`).
|
||||
- **Период опроса телеметрии**: 500 мс.
|
||||
- **Оптимизация трафика**:
|
||||
1. Публикация данных происходит **только при изменении значения** (строгое кэширование).
|
||||
2. Используется статический буфер (`intToString`/`uintToString`) вместо динамического класса `String` для экономии памяти и предотвращения фрагментации кучи.
|
||||
- **Префиксы**:
|
||||
- `.../control/...` — топики для **отправки команд** устройству (подписка).
|
||||
- `.../feedback/...` — топики для **получения статуса/телеметрии** от устройства (публикация).
|
||||
|
||||
---
|
||||
|
||||
## ⚙️ 1. Управление шаговым двигателем (Motor Control)
|
||||
|
||||
| Топик | Тип данных | Описание | Пример 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"` в feedback. | `1`, `reset` |
|
||||
|
||||
### Телеметрия двигателя (Motor Feedback)
|
||||
*(Публикуется только при изменении)*
|
||||
- `motor/feedback/rpm` (Integer): Текущая скорость.
|
||||
- `motor/feedback/totalsteps` (Integer): Общее количество шагов.
|
||||
- `motor/feedback/is_run` (String: `true`/`false`): Двигатель движется.
|
||||
- `motor/feedback/driver/status` (String: `on`/`off`): Общий статус драйвера.
|
||||
- `motor/feedback/tmc/status` (String: `on`/`off`): Статус программного включения TMC.
|
||||
|
||||
#### Детальная телеметрия TMC2209 (`motor/feedback/tmc/...`)
|
||||
- `current_percent` (Integer): Текущий % рабочего тока.
|
||||
- `microsteps` (Integer): Текущий режим микрошага.
|
||||
- `sg_result` (Integer): Текущее значение StallGuard (нагрузка).
|
||||
- `interstep_duration` (Integer): Длительность между шагами.
|
||||
- `status/over_temp` (String: `true`/`false`): Перегрев.
|
||||
- `status/short_to_ground` (String: `true`/`false`): КЗ на землю.
|
||||
- `status/open_load` (String: `true`/`false`): Обрыв нагрузки.
|
||||
- `status/stealth_chop_active` (String: `true`/`false`): Активен ли StealthChop.
|
||||
- `status/standstill` (String: `true`/`false`): Двигатель в покое.
|
||||
- `status/current_scaling` (Integer): Внутренний масштабный коэффициент тока.
|
||||
|
||||
---
|
||||
|
||||
## 🦾 2. Управление сервоприводами (Servo Control)
|
||||
|
||||
Поддержка нескольких каналов (`{channel}` от `0` до `MAX_SERVOS - 1`).
|
||||
|
||||
### Команды
|
||||
| Топик (пример для канала 0) | Тип данных | Описание | Пример Payload |
|
||||
| :--- | :---: | :--- | :--- |
|
||||
| `servo/control/0/angle` | Integer (0-180) | Установить угол поворота. | `90` |
|
||||
| `servo/control/0/enable` | String | Включить (`on`, `1`, `true`) или выключить. | `on` |
|
||||
|
||||
### Обратная связь
|
||||
- `servo/0/feedback/status` (String: `on`/`off`): Статус питания сервопривода.
|
||||
- `servo/0/feedback/angle` (Integer): Текущий установленный угол.
|
||||
|
||||
---
|
||||
|
||||
## 📏 3. Управление датчиками VL53L0X (Sensor Control) **(НОВОЕ)**
|
||||
|
||||
Поддержка до 8 каналов (`VL53L0X_MAX_CHANNELS = 8`). Топики используют параметр `{channel}` (0–7).
|
||||
|
||||
| Топик | Тип данных | Описание | Пример Payload |
|
||||
| :--- | :---: | :--- | :--- |
|
||||
| `sensor/control/mode` | Integer | Установка глобального режима измерения (0 до `MODE_COUNT - 1`). | `0`, `1` |
|
||||
| `sensor/control/mode_name` | String | *Заглушка/Логирование.* Принимает имя режима для отладки. | `LongRange` |
|
||||
| `sensor/control/calibrate/start/{ch}`| Integer | Начало калибровки: указать близкое расстояние в мм. | `50` |
|
||||
| `sensor/control/calibrate/finish/{ch}`| Integer | Завершение калибровки: указать дальнее расстояние в мм. | `500` |
|
||||
| `sensor/control/clear_cal/{ch}` | Any | Сбросить калибровку для указанного канала. | `1` |
|
||||
| `sensor/control/enable/{ch}` | String | Включить (`on`, `1`, `true`) или выключить конкретный канал. | `on` |
|
||||
| `sensor/control/publish_all` | Any | **Принудительный сброс кэша.** Заставляет устройство немедленно опубликовать текущие значения всех датчиков, даже если они не изменились. | `1` |
|
||||
|
||||
---
|
||||
|
||||
## 📊 4. Телеметрия датчиков VL53L0X (Sensor Feedback) **(НОВОЕ)**
|
||||
|
||||
### Глобальная информация о режиме
|
||||
Публикуется только при смене режима измерения:
|
||||
- `sensor/feedback/mode` (String): Человекочитаемое имя текущего режима (например, "Default", "LongRange").
|
||||
- `sensor/feedback/mode_id` (Integer): Числовой ID текущего режима.
|
||||
- `sensor/feedback/max_range` (Integer): Максимальная дальность для текущего режима (в мм).
|
||||
|
||||
### Постатусная информация по каналам (`{channel}` = 0..7)
|
||||
*(Публикуется только при изменении состояния или значения)*
|
||||
|
||||
| Топик (пример для канала 0) | Тип данных | Описание |
|
||||
| :--- | :---: | :--- |
|
||||
| `sensor/feedback/0/status` | String (`on`/`off`) | Включен ли логически данный канал. |
|
||||
| `sensor/feedback/0/calibrated` | String (`true`/`false`)| Была ли проведена калибровка для этого канала. |
|
||||
| `sensor/feedback/0/distance` | Integer или String | **Калиброванное** расстояние в мм. Если значение `65535` (ошибка/вне диапазона), публикуется строка `"out_of_range"`. |
|
||||
| `sensor/feedback/0/raw` | Integer | **Сырое** (некалиброванное) значение расстояния в мм. |
|
||||
|
||||
*Примечание: Топики `distance` и `raw` публикуются только если канал активен (`vl53l0xIsChannelActive`).*
|
||||
|
||||
---
|
||||
|
||||
## 💡 Важные особенности реализации (Обновлено)
|
||||
|
||||
1. **Безопасная работа со строками**: В новом коде добавлены функции `intToString` и `uintToString`, использующие статические буферы. Это полностью устраняет риск фрагментации памяти (heap fragmentation) при частой публикации телеметрии, который был присущ использованию класса `String`.
|
||||
2. **Принудительная публикация**: Топик `sensor/control/publish_all` сбрасывает кэш значений `distance` и `raw` на `65535`. При следующем цикле (через 500 мс) система "увидит" изменение и гарантированно отправит актуальные данные. Это полезно при подключении нового клиента, которому нужно получить текущее состояние без перезагрузки устройства.
|
||||
3. **Обработка ошибок дальности**: Если датчик возвращает `65535` (стандартный код ошибки "вне диапазона" или сбоя измерения для VL53L0X), в топик `distance` публикуется понятная строка `"out_of_range"`, а не число, что упрощает обработку на стороне клиента.
|
||||
4. **Изоляция каналов**: Цикл телеметрии VL53L0X предварительно проверяет `vl53l0xIsChannelPresent(ch)`, поэтому несуществующие или отключенные на аппаратном уровне каналы не создают лишнего трафика.
|
||||
|
||||
---
|
||||
|
||||
### 🛠️ Примеры использования (CLI / mosquitto)
|
||||
|
||||
```bash
|
||||
# --- Двигатель ---
|
||||
mosquitto_pub -t "motor/control/rpm" -m "100"
|
||||
mosquitto_pub -t "motor/control/tmc/stealthchop" -m "on"
|
||||
|
||||
# --- Сервопривод (канал 0) ---
|
||||
mosquitto_pub -t "servo/control/0/enable" -m "on"
|
||||
mosquitto_pub -t "servo/control/0/angle" -m "90"
|
||||
|
||||
# --- Датчики VL53L0X ---
|
||||
# Включить канал 1
|
||||
mosquitto_pub -t "sensor/control/enable/1" -m "on"
|
||||
|
||||
# Начать калибровку канала 1 (близкая точка 50 мм)
|
||||
mosquitto_pub -t "sensor/control/calibrate/start/1" -m "50"
|
||||
# ... передвинуть объект ...
|
||||
# Завершить калибровку канала 1 (дальняя точка 400 мм)
|
||||
mosquitto_pub -t "sensor/control/calibrate/finish/1" -m "400"
|
||||
|
||||
# Принудительно запросить публикацию всех текущих показаний датчиков
|
||||
mosquitto_pub -t "sensor/control/publish_all" -m "1"
|
||||
|
||||
# Подписаться на все события датчиков для отладки
|
||||
mosquitto_sub -t "sensor/feedback/#" -v
|
||||
```
|
||||
37
arduino_code/Test/include/README
Normal file
37
arduino_code/Test/include/README
Normal file
@@ -0,0 +1,37 @@
|
||||
|
||||
This directory is intended for project header files.
|
||||
|
||||
A header file is a file containing C declarations and macro definitions
|
||||
to be shared between several project source files. You request the use of a
|
||||
header file in your project source file (C, C++, etc) located in `src` folder
|
||||
by including it, with the C preprocessing directive `#include'.
|
||||
|
||||
```src/main.c
|
||||
|
||||
#include "header.h"
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
```
|
||||
|
||||
Including a header file produces the same results as copying the header file
|
||||
into each source file that needs it. Such copying would be time-consuming
|
||||
and error-prone. With a header file, the related declarations appear
|
||||
in only one place. If they need to be changed, they can be changed in one
|
||||
place, and programs that include the header file will automatically use the
|
||||
new version when next recompiled. The header file eliminates the labor of
|
||||
finding and changing all the copies as well as the risk that a failure to
|
||||
find one copy will result in inconsistencies within a program.
|
||||
|
||||
In C, the convention is to give header files names that end with `.h'.
|
||||
|
||||
Read more about using header files in official GCC documentation:
|
||||
|
||||
* Include Syntax
|
||||
* Include Operation
|
||||
* Once-Only Headers
|
||||
* Computed Includes
|
||||
|
||||
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
|
||||
46
arduino_code/Test/lib/README
Normal file
46
arduino_code/Test/lib/README
Normal file
@@ -0,0 +1,46 @@
|
||||
|
||||
This directory is intended for project specific (private) libraries.
|
||||
PlatformIO will compile them to static libraries and link into the executable file.
|
||||
|
||||
The source code of each library should be placed in a separate directory
|
||||
("lib/your_library_name/[Code]").
|
||||
|
||||
For example, see the structure of the following example libraries `Foo` and `Bar`:
|
||||
|
||||
|--lib
|
||||
| |
|
||||
| |--Bar
|
||||
| | |--docs
|
||||
| | |--examples
|
||||
| | |--src
|
||||
| | |- Bar.c
|
||||
| | |- Bar.h
|
||||
| | |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
|
||||
| |
|
||||
| |--Foo
|
||||
| | |- Foo.c
|
||||
| | |- Foo.h
|
||||
| |
|
||||
| |- README --> THIS FILE
|
||||
|
|
||||
|- platformio.ini
|
||||
|--src
|
||||
|- main.c
|
||||
|
||||
Example contents of `src/main.c` using Foo and Bar:
|
||||
```
|
||||
#include <Foo.h>
|
||||
#include <Bar.h>
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
The PlatformIO Library Dependency Finder will find automatically dependent
|
||||
libraries by scanning project source files.
|
||||
|
||||
More information about PlatformIO Library Dependency Finder
|
||||
- https://docs.platformio.org/page/librarymanager/ldf.html
|
||||
23
arduino_code/Test/platformio.ini
Normal file
23
arduino_code/Test/platformio.ini
Normal file
@@ -0,0 +1,23 @@
|
||||
; PlatformIO Project Configuration File
|
||||
;
|
||||
; Build options: build flags, source filter
|
||||
; Upload options: custom upload port, speed and extra flags
|
||||
; Library options: dependencies, extra library storages
|
||||
; Advanced options: extra scripting
|
||||
;
|
||||
; Please visit documentation for the other options and examples
|
||||
; https://docs.platformio.org/page/projectconf.html
|
||||
|
||||
[env:upesy_wroom]
|
||||
platform = espressif32
|
||||
board = upesy_wroom
|
||||
framework = arduino
|
||||
monitor_speed = 115200
|
||||
upload_speed = 921600
|
||||
upload_port = /dev/ttyUSB0
|
||||
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
|
||||
pololu/VL53L0X@^1.3.1
|
||||
38
arduino_code/Test/src/config.cpp
Normal file
38
arduino_code/Test/src/config.cpp
Normal file
@@ -0,0 +1,38 @@
|
||||
#include "config.h"
|
||||
|
||||
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";
|
||||
const char* MQTT_CLIENT_ID = "ESP32_Stepper";
|
||||
|
||||
// UART2: RX=16, TX=17
|
||||
HardwareSerial& TMC_SERIAL = Serial2;
|
||||
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 = 18;
|
||||
|
||||
const int STEPS_PER_REVOLUTION = 200;
|
||||
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;
|
||||
|
||||
// I2C Настройка
|
||||
const int16_t I2C_SDA_PIN = 21;
|
||||
const int16_t I2C_SCL_PIN = 22;
|
||||
|
||||
|
||||
// ========== SERVO CONFIGURATION ==========
|
||||
#define SERVO_DEFAULT_CHANNEL 0
|
||||
#define SERVO_MIN_ANGLE 0
|
||||
#define SERVO_MAX_ANGLE 180
|
||||
38
arduino_code/Test/src/config.h
Normal file
38
arduino_code/Test/src/config.h
Normal file
@@ -0,0 +1,38 @@
|
||||
#ifndef CONFIG_H
|
||||
#define CONFIG_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <Arduino.h>
|
||||
|
||||
// WiFi & MQTT
|
||||
extern const char* WIFI_SSID;
|
||||
extern const char* WIFI_PASS;
|
||||
extern const char* MQTT_SERVER;
|
||||
extern const int MQTT_PORT;
|
||||
extern const char* MQTT_USER;
|
||||
extern const char* MQTT_PASS;
|
||||
extern const char* MQTT_CLIENT_ID;
|
||||
|
||||
// UART for TMC2209
|
||||
extern HardwareSerial& TMC_SERIAL;
|
||||
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;
|
||||
|
||||
// Motor Params
|
||||
extern const int STEPS_PER_REVOLUTION; // Базовые шаги мотора (обычно 200)
|
||||
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;
|
||||
|
||||
// I2C Настройка
|
||||
extern const int16_t I2C_SDA_PIN;
|
||||
extern const int16_t I2C_SCL_PIN;
|
||||
|
||||
#endif
|
||||
42
arduino_code/Test/src/main.cpp
Normal file
42
arduino_code/Test/src/main.cpp
Normal file
@@ -0,0 +1,42 @@
|
||||
#include <Arduino.h>
|
||||
#include <TMC2209.h>
|
||||
#include "config.h"
|
||||
#include "motor.h"
|
||||
#include "vl53l0x_sensor.h"
|
||||
#include "mqtt_handler.h"
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
|
||||
// Инициализация мотора (Core 1 context initially)
|
||||
motorInit();
|
||||
|
||||
xTaskCreatePinnedToCore(
|
||||
sensorTask,
|
||||
"SensorTask",
|
||||
4096,
|
||||
NULL,
|
||||
1,
|
||||
NULL,
|
||||
0
|
||||
);
|
||||
|
||||
// Запуск задачи MQTT на Core 0
|
||||
xTaskCreatePinnedToCore(
|
||||
mqttTask,
|
||||
"MQTT_Task",
|
||||
20480,
|
||||
NULL,
|
||||
3,
|
||||
NULL,
|
||||
0 // CORE 0
|
||||
);
|
||||
|
||||
Serial.println("System Initialized. Multi-core ready.");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// Loop выполняется на Core 1
|
||||
motorLoop();
|
||||
vTaskDelay(pdMS_TO_TICKS(5));
|
||||
}
|
||||
295
arduino_code/Test/src/motor.cpp
Normal file
295
arduino_code/Test/src/motor.cpp
Normal file
@@ -0,0 +1,295 @@
|
||||
#include "motor.h"
|
||||
#include "config.h"
|
||||
|
||||
static TMC2209 stepper_driver;
|
||||
static bool tmc_initialized = false;
|
||||
static SemaphoreHandle_t tmc_uart_mutex = NULL;
|
||||
|
||||
volatile unsigned long total_steps = 0;
|
||||
static int target_rpm = 0;
|
||||
static int current_rpm_display = 0;
|
||||
|
||||
static bool is_ramping = false;
|
||||
static unsigned long ramp_start_ms = 0;
|
||||
static float start_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; // Флаг для отправки хотя бы раз
|
||||
|
||||
static uint16_t current_microsteps = TMC_MICROSTEPS;
|
||||
static uint8_t current_run_percent = TMC_RUN_CURRENT_PERCENT;
|
||||
|
||||
#define TMC_LOCK() xSemaphoreTake(tmc_uart_mutex, portMAX_DELAY)
|
||||
#define TMC_UNLOCK() xSemaphoreGive(tmc_uart_mutex)
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void motorInit() {
|
||||
pinMode(EN_PIN, OUTPUT);
|
||||
digitalWrite(EN_PIN, LOW);
|
||||
tmc_uart_mutex = xSemaphoreCreateMutex();
|
||||
|
||||
TMC_SERIAL.begin(TMC_BAUD_RATE, SERIAL_8N1, TMC_RX_PIN, TMC_TX_PIN);
|
||||
delay(500);
|
||||
|
||||
TMC_LOCK();
|
||||
|
||||
stepper_driver.setup(TMC_SERIAL, TMC_BAUD_RATE,
|
||||
(TMC2209::SerialAddress)TMC_SERIAL_ADDRESS,
|
||||
TMC_RX_PIN, TMC_TX_PIN);
|
||||
|
||||
delay(200);
|
||||
|
||||
// Проверка связи
|
||||
if (!stepper_driver.isCommunicating()) {
|
||||
Serial.println("ERROR: TMC2209 not communicating!");
|
||||
TMC_UNLOCK();
|
||||
tmc_initialized = false;
|
||||
return;
|
||||
}
|
||||
|
||||
Serial.println("TMC2209 communicating OK");
|
||||
|
||||
// Базовая настройка
|
||||
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);
|
||||
|
||||
stepper_driver.enableAutomaticCurrentScaling();
|
||||
stepper_driver.enableAutomaticGradientAdaptation();
|
||||
|
||||
// КРИТИЧНО: Отключаем StealthChop для работы moveAtVelocity()!
|
||||
stepper_driver.disableStealthChop();
|
||||
delay(10);
|
||||
|
||||
// Включаем CoolStep для энергосбережения
|
||||
stepper_driver.enableCoolStep();
|
||||
|
||||
// Программное включение драйвера
|
||||
stepper_driver.enable();
|
||||
delay(100);
|
||||
|
||||
tmc_initialized = stepper_driver.isSetupAndCommunicating();
|
||||
|
||||
if (tmc_initialized) {
|
||||
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() {
|
||||
if (is_ramping) {
|
||||
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);
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(5));
|
||||
}
|
||||
|
||||
void setTargetRPM(int rpm) {
|
||||
target_rpm = rpm;
|
||||
if (rpm != 0 && current_rpm_display < 5) resetSteps();
|
||||
|
||||
unsigned long steps_per_rev = (unsigned long)STEPS_PER_REVOLUTION * current_microsteps;
|
||||
|
||||
start_speed_sps = current_speed_sps;
|
||||
end_speed_sps = (rpm != 0) ? ((float)rpm * steps_per_rev) / 60.0f : 0;
|
||||
|
||||
ramp_start_ms = millis();
|
||||
is_ramping = 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;
|
||||
}
|
||||
|
||||
// Функция проверки состояния EN_PIN
|
||||
bool checkDriverStatus() {
|
||||
bool en_state = (digitalRead(EN_PIN) == LOW);
|
||||
return en_state;
|
||||
}
|
||||
|
||||
// Функция проверки программного состояния TMC
|
||||
bool checkTmcSoftwareEnable() {
|
||||
TMC2209::Settings s = tmcGetSettings();
|
||||
bool tmc_state = s.software_enabled;
|
||||
return tmc_state;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
42
arduino_code/Test/src/motor.h
Normal file
42
arduino_code/Test/src/motor.h
Normal file
@@ -0,0 +1,42 @@
|
||||
#ifndef MOTOR_H
|
||||
#define MOTOR_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <TMC2209.h>
|
||||
|
||||
void motorInit();
|
||||
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();
|
||||
bool checkDriverStatus();
|
||||
bool checkTmcSoftwareEnable();
|
||||
|
||||
#endif
|
||||
506
arduino_code/Test/src/mqtt_handler.cpp
Normal file
506
arduino_code/Test/src/mqtt_handler.cpp
Normal file
@@ -0,0 +1,506 @@
|
||||
#include "mqtt_handler.h"
|
||||
#include "config.h"
|
||||
#include "motor.h"
|
||||
#include "servo_control.h"
|
||||
#include "vl53l0x_sensor.h"
|
||||
|
||||
static WiFiClient espClient;
|
||||
static PubSubClient client(espClient);
|
||||
|
||||
// Кэш телеметрии
|
||||
static unsigned long last_feedback_time = 0;
|
||||
static int last_pub_rpm = -1;
|
||||
static unsigned long last_pub_steps = -1;
|
||||
static int last_pub_is_run = -1;
|
||||
|
||||
// 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 int last_pub_driver_status = -1;
|
||||
static int last_pub_tmc_software_enable = -1;
|
||||
static uint8_t last_pub_current_scaling = 255;
|
||||
|
||||
// ============================================
|
||||
// КЭШИРОВАНИЕ ДЛЯ VL53L0X
|
||||
// ============================================
|
||||
|
||||
#define VL53L0X_MAX_CHANNELS 8
|
||||
static uint16_t last_pub_vl53_distance[VL53L0X_MAX_CHANNELS] = {65535};
|
||||
static uint16_t last_pub_vl53_raw[VL53L0X_MAX_CHANNELS] = {65535};
|
||||
static int last_pub_vl53_status[VL53L0X_MAX_CHANNELS] = {-1};
|
||||
static int last_pub_vl53_calibrated[VL53L0X_MAX_CHANNELS] = {-1};
|
||||
static MeasurementMode last_pub_vl53_mode = MODE_COUNT;
|
||||
|
||||
// ============================================
|
||||
// БУФЕРЫ ДЛЯ ПРЕОБРАЗОВАНИЯ
|
||||
// ============================================
|
||||
|
||||
static char int_buffer[16];
|
||||
static char uint_buffer[16];
|
||||
|
||||
static const char* intToString(int value) {
|
||||
snprintf(int_buffer, sizeof(int_buffer), "%d", value);
|
||||
return int_buffer;
|
||||
}
|
||||
|
||||
static const char* uintToString(unsigned long value) {
|
||||
snprintf(uint_buffer, sizeof(uint_buffer), "%lu", value);
|
||||
return uint_buffer;
|
||||
}
|
||||
|
||||
// ============================================
|
||||
// WIFI И MQTT ПОДКЛЮЧЕНИЕ
|
||||
// ============================================
|
||||
|
||||
static void setup_wifi() {
|
||||
Serial.print("Connecting to WiFi");
|
||||
WiFi.begin(WIFI_SSID, WIFI_PASS);
|
||||
while (WiFi.status() != WL_CONNECTED) {
|
||||
vTaskDelay(pdMS_TO_TICKS(500));
|
||||
Serial.print(".");
|
||||
}
|
||||
Serial.println("\nWiFi Connected");
|
||||
}
|
||||
|
||||
static void resetAllCaches() {
|
||||
// Motor
|
||||
last_pub_rpm = -1;
|
||||
last_pub_steps = (unsigned long)-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_driver_status = -1;
|
||||
last_pub_tmc_software_enable = -1;
|
||||
|
||||
// VL53L0X
|
||||
for (int i = 0; i < VL53L0X_MAX_CHANNELS; i++) {
|
||||
last_pub_vl53_distance[i] = 65535;
|
||||
last_pub_vl53_raw[i] = 65535;
|
||||
last_pub_vl53_status[i] = -1;
|
||||
last_pub_vl53_calibrated[i] = -1;
|
||||
}
|
||||
last_pub_vl53_mode = MODE_COUNT;
|
||||
}
|
||||
|
||||
static void subscribeToAllTopics() {
|
||||
// Motor
|
||||
client.subscribe("motor/control/rpm");
|
||||
client.subscribe("motor/control/driver");
|
||||
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");
|
||||
|
||||
// Servo
|
||||
client.subscribe("servo/control/+/#");
|
||||
|
||||
// VL53L0X
|
||||
client.subscribe("sensor/control/mode");
|
||||
client.subscribe("sensor/control/mode_name");
|
||||
client.subscribe("sensor/control/calibrate/start/+");
|
||||
client.subscribe("sensor/control/calibrate/finish/+");
|
||||
client.subscribe("sensor/control/clear_cal/+");
|
||||
client.subscribe("sensor/control/enable/+");
|
||||
client.subscribe("sensor/control/publish_all");
|
||||
}
|
||||
|
||||
static void reconnect() {
|
||||
while (!client.connected()) {
|
||||
if (client.connect(MQTT_CLIENT_ID, MQTT_USER, MQTT_PASS)) {
|
||||
resetAllCaches();
|
||||
subscribeToAllTopics();
|
||||
Serial.println("MQTT Connected and subscribed");
|
||||
} else {
|
||||
Serial.printf("MQTT connection failed, rc=%d, retrying...\n", client.state());
|
||||
vTaskDelay(pdMS_TO_TICKS(5000));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================
|
||||
// ОБРАБОТКА ВХОДЯЩИХ MQTT КОМАНД
|
||||
// ============================================
|
||||
static void callback(char* topic, byte* payload, unsigned int length) {
|
||||
char msg[length + 1];
|
||||
memcpy(msg, payload, length);
|
||||
msg[length] = '\0';
|
||||
|
||||
if (strcmp(topic, "motor/control/rpm") == 0) {
|
||||
setTargetRPM(atoi(msg)); // Поддерживает отрицательные для реверса!
|
||||
}
|
||||
else if (strcmp(topic, "motor/control/driver") == 0) {
|
||||
// TMC2209: LOW = Enabled, HIGH = Disabled
|
||||
bool enable = (strcmp(msg, "on") == 0);
|
||||
digitalWrite(EN_PIN, enable ? LOW : HIGH);
|
||||
}
|
||||
else if (strcmp(topic, "motor/control/totalsteps/reset") == 0) {
|
||||
resetSteps();
|
||||
if (client.connected()) client.publish("motor/feedback/totalsteps", "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);
|
||||
}
|
||||
else if (strncmp(topic, "servo/control", 12) == 0) {
|
||||
handleServoMQTTCommand(topic, msg);
|
||||
}
|
||||
// === VL53L0X ===
|
||||
else if (strcmp(topic, "sensor/control/mode") == 0) {
|
||||
int mode = atoi(msg);
|
||||
if (mode >= 0 && mode < MODE_COUNT) {
|
||||
vl53l0xSetMode((MeasurementMode)mode);
|
||||
}
|
||||
}
|
||||
else if (strcmp(topic, "sensor/control/mode_name") == 0) {
|
||||
// Маппинг имени режима на ID (если нужно)
|
||||
// Пока просто логируем
|
||||
Serial.printf("Mode name request: %s\n", msg);
|
||||
}
|
||||
else if (strncmp(topic, "sensor/control/calibrate/start/", 31) == 0) {
|
||||
int channel = atoi(topic + 31);
|
||||
int near_mm = atoi(msg);
|
||||
if (channel >= 0 && channel < VL53L0X_MAX_CHANNELS && near_mm > 0) {
|
||||
vl53l0xStartCalibration(channel, near_mm);
|
||||
}
|
||||
}
|
||||
else if (strncmp(topic, "sensor/control/calibrate/finish/", 32) == 0) {
|
||||
int channel = atoi(topic + 32);
|
||||
int far_mm = atoi(msg);
|
||||
if (channel >= 0 && channel < VL53L0X_MAX_CHANNELS && far_mm > 0) {
|
||||
vl53l0xFinishCalibration(channel, far_mm);
|
||||
}
|
||||
}
|
||||
else if (strncmp(topic, "sensor/control/clear_cal/", 25) == 0) {
|
||||
int channel = atoi(topic + 25);
|
||||
if (channel >= 0 && channel < VL53L0X_MAX_CHANNELS) {
|
||||
vl53l0xClearCalibration(channel);
|
||||
}
|
||||
}
|
||||
else if (strncmp(topic, "sensor/control/enable/", 22) == 0) {
|
||||
int channel = atoi(topic + 22);
|
||||
bool enable = (strcmp(msg, "on") == 0 || strcmp(msg, "1") == 0 || strcmp(msg, "true") == 0);
|
||||
if (channel >= 0 && channel < VL53L0X_MAX_CHANNELS) {
|
||||
vl53l0xEnableChannel(channel, enable);
|
||||
}
|
||||
}
|
||||
else if (strcmp(topic, "sensor/control/publish_all") == 0) {
|
||||
// Сбрасываем кэш для принудительной публикации
|
||||
for (int i = 0; i < VL53L0X_MAX_CHANNELS; i++) {
|
||||
last_pub_vl53_distance[i] = 65535;
|
||||
last_pub_vl53_raw[i] = 65535;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================
|
||||
// ПУБЛИКАЦИЯ MOTOR TELEMETRY
|
||||
// ============================================
|
||||
|
||||
static void publishMotorTelemetry() {
|
||||
// RPM
|
||||
int rpm = getCurrentRPM();
|
||||
if (rpm != last_pub_rpm) {
|
||||
client.publish("motor/feedback/rpm", intToString(rpm));
|
||||
last_pub_rpm = rpm;
|
||||
}
|
||||
|
||||
// Steps
|
||||
unsigned long steps = getMotorSteps();
|
||||
if (steps != last_pub_steps) {
|
||||
client.publish("motor/feedback/totalsteps", uintToString(steps));
|
||||
last_pub_steps = steps;
|
||||
}
|
||||
|
||||
// Is running
|
||||
int run = isMotorRunning() ? 1 : 0;
|
||||
if (run != last_pub_is_run) {
|
||||
client.publish("motor/feedback/is_run", run ? "true" : "false");
|
||||
last_pub_is_run = run;
|
||||
}
|
||||
|
||||
// TMC
|
||||
if (tmcIsInitialized()) {
|
||||
uint8_t pct = tmcGetRunCurrentPercent();
|
||||
if (pct != last_pub_current_pct) {
|
||||
client.publish("motor/feedback/tmc/current_percent", intToString(pct));
|
||||
last_pub_current_pct = pct;
|
||||
}
|
||||
|
||||
uint16_t ms = tmcGetMicrostepsSetting();
|
||||
if (ms != last_pub_microsteps) {
|
||||
client.publish("motor/feedback/tmc/microsteps", intToString(ms));
|
||||
last_pub_microsteps = ms;
|
||||
}
|
||||
|
||||
uint16_t sg = tmcGetStallGuardResult();
|
||||
if (sg != last_pub_sg) {
|
||||
client.publish("motor/feedback/tmc/sg_result", intToString(sg));
|
||||
last_pub_sg = sg;
|
||||
}
|
||||
|
||||
uint32_t interstep = tmcGetInterstepDuration();
|
||||
if (interstep != last_pub_interstep) {
|
||||
client.publish("motor/feedback/tmc/interstep_duration", uintToString(interstep));
|
||||
last_pub_interstep = interstep;
|
||||
}
|
||||
|
||||
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", intToString(status.current_scaling));
|
||||
last_pub_current_scaling = status.current_scaling;
|
||||
}
|
||||
|
||||
int cds = checkDriverStatus() ? 1 : 0;
|
||||
if (cds != last_pub_driver_status) {
|
||||
client.publish("motor/feedback/driver/status", cds ? "on" : "off");
|
||||
last_pub_driver_status = cds;
|
||||
}
|
||||
|
||||
int tse = checkTmcSoftwareEnable() ? 1 : 0;
|
||||
if (tse != last_pub_tmc_software_enable) {
|
||||
client.publish("motor/feedback/tmc/status", tse ? "on" : "off");
|
||||
last_pub_tmc_software_enable = tse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================
|
||||
// ПУБЛИКАЦИЯ VL53L0X TELEMETRY
|
||||
// ============================================
|
||||
|
||||
static void publishVL53L0XTelemetry() {
|
||||
// Публикация режима
|
||||
MeasurementMode current_mode = vl53l0xGetMode();
|
||||
if (current_mode != last_pub_vl53_mode) {
|
||||
const ModeProfile* profile = vl53l0xGetModeProfile();
|
||||
|
||||
client.publish("sensor/feedback/mode", profile->name);
|
||||
client.publish("sensor/feedback/mode_id", intToString(current_mode));
|
||||
client.publish("sensor/feedback/max_range", intToString(profile->max_range_mm));
|
||||
|
||||
last_pub_vl53_mode = current_mode;
|
||||
}
|
||||
|
||||
// Публикация данных с каждого канала
|
||||
for (uint8_t ch = 0; ch < VL53L0X_MAX_CHANNELS; ch++) {
|
||||
if (!vl53l0xIsChannelPresent(ch)) continue;
|
||||
|
||||
char topic[64];
|
||||
|
||||
// Статус канала
|
||||
int status = vl53l0xIsChannelEnabled(ch) ? 1 : 0;
|
||||
if (status != last_pub_vl53_status[ch]) {
|
||||
snprintf(topic, sizeof(topic), "sensor/feedback/%d/status", ch);
|
||||
client.publish(topic, status ? "on" : "off");
|
||||
last_pub_vl53_status[ch] = status;
|
||||
}
|
||||
|
||||
// Статус калибровки
|
||||
int calibrated = vl53l0xIsCalibrated(ch) ? 1 : 0;
|
||||
if (calibrated != last_pub_vl53_calibrated[ch]) {
|
||||
snprintf(topic, sizeof(topic), "sensor/feedback/%d/calibrated", ch);
|
||||
client.publish(topic, calibrated ? "true" : "false");
|
||||
last_pub_vl53_calibrated[ch] = calibrated;
|
||||
}
|
||||
|
||||
// Только если канал активен, публикуем расстояния
|
||||
if (vl53l0xIsChannelActive(ch)) {
|
||||
// Калиброванное расстояние
|
||||
uint16_t distance = vl53l0xReadDistance(ch);
|
||||
if (distance != last_pub_vl53_distance[ch]) {
|
||||
snprintf(topic, sizeof(topic), "sensor/feedback/%d/distance", ch);
|
||||
if (distance != 65535) {
|
||||
client.publish(topic, intToString(distance));
|
||||
} else {
|
||||
client.publish(topic, "out_of_range");
|
||||
}
|
||||
last_pub_vl53_distance[ch] = distance;
|
||||
}
|
||||
|
||||
// Сырое значение
|
||||
uint16_t raw = vl53l0xReadRawDistance(ch);
|
||||
if (raw != last_pub_vl53_raw[ch]) {
|
||||
snprintf(topic, sizeof(topic), "sensor/feedback/%d/raw", ch);
|
||||
if (raw != 65535) {
|
||||
client.publish(topic, intToString(raw));
|
||||
}
|
||||
last_pub_vl53_raw[ch] = raw;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/////
|
||||
|
||||
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/%2d/%8s", &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);
|
||||
}
|
||||
}
|
||||
|
||||
/////
|
||||
|
||||
// ============================================
|
||||
// ГЛАВНЫЙ ЦИКЛ ПУБЛИКАЦИИ
|
||||
// ============================================
|
||||
|
||||
static void publishTelemetry() {
|
||||
unsigned long now = millis();
|
||||
if (now - last_feedback_time >= 500) {
|
||||
publishMotorTelemetry();
|
||||
publishVL53L0XTelemetry();
|
||||
last_feedback_time = now;
|
||||
}
|
||||
}
|
||||
|
||||
void mqttTask(void *parameter) {
|
||||
static unsigned long last_stack_check = 0;
|
||||
|
||||
setup_wifi();
|
||||
client.setServer(MQTT_SERVER, MQTT_PORT);
|
||||
client.setCallback(callback);
|
||||
|
||||
servoInit();
|
||||
|
||||
for (;;) {
|
||||
if (!client.connected()) reconnect();
|
||||
client.loop();
|
||||
publishTelemetry();
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(10));
|
||||
}
|
||||
}
|
||||
11
arduino_code/Test/src/mqtt_handler.h
Normal file
11
arduino_code/Test/src/mqtt_handler.h
Normal file
@@ -0,0 +1,11 @@
|
||||
#ifndef MQTT_HANDLER_H
|
||||
#define MQTT_HANDLER_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <WiFi.h>
|
||||
#include <PubSubClient.h>
|
||||
|
||||
void mqttTask(void *parameter);
|
||||
void handleServoMQTTCommand(const char* topic, const char* payload);
|
||||
|
||||
#endif
|
||||
98
arduino_code/Test/src/servo_control.cpp
Normal file
98
arduino_code/Test/src/servo_control.cpp
Normal file
@@ -0,0 +1,98 @@
|
||||
#include "servo_control.h"
|
||||
#include "mqtt_handler.h"
|
||||
#include "config.h"
|
||||
|
||||
static Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
|
||||
static bool servo_initialized = false;
|
||||
|
||||
|
||||
|
||||
void servoInit() {
|
||||
Serial.println("Initializing PCA9685 servo driver...");
|
||||
|
||||
// Инициализация I2C на пинах 21 (SDA) и 22 (SCL)
|
||||
Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
|
||||
|
||||
pwm.begin();
|
||||
pwm.setOscillatorFrequency(27000000);
|
||||
pwm.setPWMFreq(50); // 50 Hz для сервоприводов
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// Преобразование угла (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_angles[channel] = angle;
|
||||
servo_enabled[channel] = true;
|
||||
|
||||
uint16_t pulse = angleToPulse(angle);
|
||||
setServoPulse(channel, 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;
|
||||
|
||||
// Преобразование микросекунд в тики PCA9685
|
||||
// PCA9685 имеет 4096 тиков на период при 50Hz = 20000 мкс
|
||||
// 1 мкс = 4096 / 20000 = 0.2048 тика
|
||||
double pulselength = 4096.0 / 20000.0; // тиков на микросекунду
|
||||
uint16_t ticks = pulse * pulselength;
|
||||
|
||||
pwm.setPWM(channel, 0, ticks);
|
||||
}
|
||||
|
||||
void enableServo(uint8_t channel) {
|
||||
if (!servo_initialized || channel >= MAX_SERVOS) return;
|
||||
|
||||
servo_enabled[channel] = true;
|
||||
setServoAngle(channel, current_angles[channel]);
|
||||
|
||||
Serial.printf("Servo %d: ENABLED\n", channel);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
uint8_t getServoAngle(uint8_t channel) {
|
||||
if (channel >= MAX_SERVOS) return 0;
|
||||
return current_angles[channel];
|
||||
}
|
||||
|
||||
bool isServoEnabled(uint8_t channel) {
|
||||
if (channel >= MAX_SERVOS) return false;
|
||||
return servo_enabled[channel];
|
||||
}
|
||||
32
arduino_code/Test/src/servo_control.h
Normal file
32
arduino_code/Test/src/servo_control.h
Normal file
@@ -0,0 +1,32 @@
|
||||
#ifndef SERVO_H
|
||||
#define SERVO_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <Adafruit_PWMServoDriver.h>
|
||||
|
||||
|
||||
// Хранение текущего состояния сервоприводов
|
||||
#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 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);
|
||||
|
||||
// Получение состояния
|
||||
uint8_t getServoAngle(uint8_t channel);
|
||||
bool isServoEnabled(uint8_t channel);
|
||||
|
||||
#endif // SERVO_H
|
||||
477
arduino_code/Test/src/vl53l0x_sensor.cpp
Normal file
477
arduino_code/Test/src/vl53l0x_sensor.cpp
Normal file
@@ -0,0 +1,477 @@
|
||||
#include "vl53l0x_sensor.h"
|
||||
#include "config.h"
|
||||
|
||||
// ============================================
|
||||
// КОНСТАНТЫ
|
||||
// ============================================
|
||||
|
||||
#define TCA9548A_ADDRESS 0x70
|
||||
#define TCA9548A_CHANNELS 8
|
||||
|
||||
#define MIN_DISTANCE_MM 30
|
||||
#define OUT_OF_RANGE_VALUE 65535
|
||||
|
||||
#define FILTER_SIZE 5
|
||||
|
||||
// ============================================
|
||||
// ПРОФИЛИ РЕЖИМОВ
|
||||
// ============================================
|
||||
|
||||
static const ModeProfile MODES[MODE_COUNT] = {
|
||||
{
|
||||
"HIGH_ACCURACY",
|
||||
200000, 14, 10, 0.5f, 500, 2
|
||||
},
|
||||
{
|
||||
"PRECISION",
|
||||
66000, 14, 10, 0.3f, 1000, 5
|
||||
},
|
||||
{
|
||||
"DEFAULT",
|
||||
33000, 14, 10, 0.25f, 1200, 15
|
||||
},
|
||||
{
|
||||
"LONG_RANGE",
|
||||
33000, 18, 14, 0.1f, 2000, 40
|
||||
},
|
||||
{
|
||||
"ULTRA_LONG",
|
||||
100000, 18, 14, 0.05f, 2500, 80
|
||||
}
|
||||
};
|
||||
|
||||
// ============================================
|
||||
// ГЛОБАЛЬНЫЕ ПЕРЕМЕННЫЕ
|
||||
// ============================================
|
||||
|
||||
static VL53L0X sensor;
|
||||
static Preferences preferences;
|
||||
|
||||
static MeasurementMode current_mode = MODE_DEFAULT;
|
||||
static bool sensor_present[TCA9548A_CHANNELS] = {false};
|
||||
static bool channel_enabled[TCA9548A_CHANNELS] = {false};
|
||||
static CalibrationData calibration[TCA9548A_CHANNELS];
|
||||
|
||||
static uint16_t filter_buffer[TCA9548A_CHANNELS][FILTER_SIZE];
|
||||
static uint8_t filter_index[TCA9548A_CHANNELS] = {0};
|
||||
|
||||
static bool calibration_in_progress = false;
|
||||
static uint8_t calibration_channel = 0;
|
||||
static uint16_t calibration_near_raw = 0;
|
||||
static uint16_t calibration_near_known = 0;
|
||||
|
||||
// ============================================
|
||||
// TCA9548A МУЛЬТИПЛЕКСОР
|
||||
// ============================================
|
||||
|
||||
static void TCA9548A_Select(uint8_t channel) {
|
||||
Wire.beginTransmission(TCA9548A_ADDRESS);
|
||||
Wire.write((channel < TCA9548A_CHANNELS) ? (1 << channel) : 0x00);
|
||||
Wire.endTransmission();
|
||||
delay(3);
|
||||
}
|
||||
|
||||
static void TCA9548A_DisableAll() {
|
||||
Wire.beginTransmission(TCA9548A_ADDRESS);
|
||||
Wire.write(0x00);
|
||||
Wire.endTransmission();
|
||||
}
|
||||
|
||||
static bool checkTCA9548A() {
|
||||
Wire.beginTransmission(TCA9548A_ADDRESS);
|
||||
return (Wire.endTransmission() == 0);
|
||||
}
|
||||
|
||||
// ============================================
|
||||
// NVS (СОХРАНЕНИЕ В FLASH)
|
||||
// ============================================
|
||||
|
||||
static void loadCalibration() {
|
||||
preferences.begin("vl53_cal", true);
|
||||
|
||||
for (uint8_t ch = 0; ch < TCA9548A_CHANNELS; ch++) {
|
||||
char key[16];
|
||||
snprintf(key, sizeof(key), "ch%d", ch);
|
||||
|
||||
size_t len = preferences.getBytesLength(key);
|
||||
if (len == sizeof(CalibrationData)) {
|
||||
preferences.getBytes(key, &calibration[ch], sizeof(CalibrationData));
|
||||
} else {
|
||||
calibration[ch].valid = false;
|
||||
calibration[ch].scale = 1.0f;
|
||||
calibration[ch].offset = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
int saved_mode = preferences.getInt("mode", MODE_DEFAULT);
|
||||
if (saved_mode >= 0 && saved_mode < MODE_COUNT) {
|
||||
current_mode = (MeasurementMode)saved_mode;
|
||||
}
|
||||
|
||||
preferences.end();
|
||||
}
|
||||
|
||||
static void saveCalibration(uint8_t channel) {
|
||||
preferences.begin("vl53_cal", false);
|
||||
char key[16];
|
||||
snprintf(key, sizeof(key), "ch%d", channel);
|
||||
preferences.putBytes(key, &calibration[channel], sizeof(CalibrationData));
|
||||
preferences.end();
|
||||
}
|
||||
|
||||
static void saveMode() {
|
||||
preferences.begin("vl53_cal", false);
|
||||
preferences.putInt("mode", (int)current_mode);
|
||||
preferences.end();
|
||||
}
|
||||
|
||||
// ============================================
|
||||
// ПРИМЕНЕНИЕ РЕЖИМА
|
||||
// ============================================
|
||||
|
||||
static bool applyModeProfile() {
|
||||
const ModeProfile& profile = MODES[current_mode];
|
||||
|
||||
sensor.setTimeout(500);
|
||||
|
||||
if (!sensor.init()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
sensor.setAddress(0x29);
|
||||
sensor.setSignalRateLimit(profile.signal_rate_limit);
|
||||
|
||||
sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, profile.vcsel_prerange);
|
||||
sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, profile.vcsel_final);
|
||||
sensor.setMeasurementTimingBudget(profile.timing_budget_us);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ============================================
|
||||
// КАЛИБРОВКА И ФИЛЬТРАЦИЯ
|
||||
// ============================================
|
||||
|
||||
static uint16_t applyCalibration(uint8_t channel, uint16_t raw_mm) {
|
||||
if (!calibration[channel].valid || raw_mm == OUT_OF_RANGE_VALUE) {
|
||||
return raw_mm;
|
||||
}
|
||||
|
||||
float calibrated = (float)raw_mm * calibration[channel].scale + calibration[channel].offset;
|
||||
|
||||
const ModeProfile& profile = MODES[current_mode];
|
||||
if (calibrated < MIN_DISTANCE_MM) calibrated = MIN_DISTANCE_MM;
|
||||
if (calibrated > profile.max_range_mm) return OUT_OF_RANGE_VALUE;
|
||||
|
||||
return (uint16_t)calibrated;
|
||||
}
|
||||
|
||||
static uint16_t applyFilter(uint8_t channel, uint16_t new_value) {
|
||||
if (new_value == OUT_OF_RANGE_VALUE) return OUT_OF_RANGE_VALUE;
|
||||
|
||||
filter_buffer[channel][filter_index[channel]] = new_value;
|
||||
filter_index[channel] = (filter_index[channel] + 1) % FILTER_SIZE;
|
||||
|
||||
uint32_t sum = 0;
|
||||
uint8_t count = 0;
|
||||
for (uint8_t i = 0; i < FILTER_SIZE; i++) {
|
||||
if (filter_buffer[channel][i] != 0) {
|
||||
sum += filter_buffer[channel][i];
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
return (count > 0) ? (uint16_t)(sum / count) : new_value;
|
||||
}
|
||||
|
||||
// ============================================
|
||||
// ПУБЛИЧНЫЕ ФУНКЦИИ
|
||||
// ============================================
|
||||
|
||||
void vl53l0xInit() {
|
||||
Serial.println("Initializing VL53L0X sensors...");
|
||||
|
||||
Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
|
||||
Wire.setClock(400000);
|
||||
delay(100);
|
||||
|
||||
if (!checkTCA9548A()) {
|
||||
Serial.println("ERROR: TCA9548A not found!");
|
||||
return;
|
||||
}
|
||||
Serial.println("✓ TCA9548A found");
|
||||
TCA9548A_DisableAll();
|
||||
|
||||
loadCalibration();
|
||||
Serial.printf("✓ Loaded mode: %s\n", MODES[current_mode].name);
|
||||
|
||||
Serial.println("\nScanning VL53L0X channels:");
|
||||
for (uint8_t channel = 0; channel < TCA9548A_CHANNELS; channel++) {
|
||||
TCA9548A_Select(channel);
|
||||
delay(20);
|
||||
|
||||
if (applyModeProfile()) {
|
||||
sensor_present[channel] = true;
|
||||
channel_enabled[channel] = true;
|
||||
Serial.printf(" ✓ Channel %d: VL53L0X found", channel);
|
||||
if (calibration[channel].valid) Serial.print(" [CALIBRATED]");
|
||||
Serial.println();
|
||||
|
||||
memset(filter_buffer[channel], 0, sizeof(filter_buffer[channel]));
|
||||
filter_index[channel] = 0;
|
||||
} else {
|
||||
sensor_present[channel] = false;
|
||||
channel_enabled[channel] = false;
|
||||
Serial.printf(" ✗ Channel %d: No device\n", channel);
|
||||
}
|
||||
|
||||
TCA9548A_DisableAll();
|
||||
delay(5);
|
||||
}
|
||||
|
||||
Serial.printf("\n✓ VL53L0X initialized: %d sensors found\n\n",
|
||||
vl53l0xGetChannelCount());
|
||||
}
|
||||
|
||||
void vl53l0xLoop() {
|
||||
// Внутренняя логика датчиков (если нужна)
|
||||
// Сейчас вся публикация в mqtt_handle.cpp
|
||||
}
|
||||
|
||||
bool vl53l0xSetMode(MeasurementMode mode) {
|
||||
if (mode < 0 || mode >= MODE_COUNT) {
|
||||
Serial.printf("ERROR: Invalid mode %d\n", mode);
|
||||
return false;
|
||||
}
|
||||
|
||||
current_mode = mode;
|
||||
saveMode();
|
||||
|
||||
const ModeProfile& profile = MODES[current_mode];
|
||||
Serial.printf("✓ Mode changed to: %s (max %d mm)\n",
|
||||
profile.name, profile.max_range_mm);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
MeasurementMode vl53l0xGetMode() {
|
||||
return current_mode;
|
||||
}
|
||||
|
||||
const ModeProfile* vl53l0xGetModeProfile() {
|
||||
return &MODES[current_mode];
|
||||
}
|
||||
|
||||
uint16_t vl53l0xReadDistance(uint8_t channel) {
|
||||
if (channel >= TCA9548A_CHANNELS || !sensor_present[channel] || !channel_enabled[channel]) {
|
||||
return OUT_OF_RANGE_VALUE;
|
||||
}
|
||||
|
||||
TCA9548A_Select(channel);
|
||||
|
||||
if (!applyModeProfile()) {
|
||||
TCA9548A_DisableAll();
|
||||
return OUT_OF_RANGE_VALUE;
|
||||
}
|
||||
|
||||
uint16_t distance = sensor.readRangeSingleMillimeters();
|
||||
bool timeout = sensor.timeoutOccurred();
|
||||
|
||||
TCA9548A_DisableAll();
|
||||
|
||||
if (distance == 65535 || timeout) return OUT_OF_RANGE_VALUE;
|
||||
|
||||
const ModeProfile& profile = MODES[current_mode];
|
||||
if (distance < MIN_DISTANCE_MM || distance > profile.max_range_mm) {
|
||||
return OUT_OF_RANGE_VALUE;
|
||||
}
|
||||
|
||||
uint16_t filtered = applyFilter(channel, distance);
|
||||
uint16_t calibrated = applyCalibration(channel, filtered);
|
||||
|
||||
return calibrated;
|
||||
}
|
||||
|
||||
uint16_t vl53l0xReadRawDistance(uint8_t channel) {
|
||||
if (channel >= TCA9548A_CHANNELS || !sensor_present[channel] || !channel_enabled[channel]) {
|
||||
return OUT_OF_RANGE_VALUE;
|
||||
}
|
||||
|
||||
TCA9548A_Select(channel);
|
||||
|
||||
if (!applyModeProfile()) {
|
||||
TCA9548A_DisableAll();
|
||||
return OUT_OF_RANGE_VALUE;
|
||||
}
|
||||
|
||||
uint16_t distance = sensor.readRangeSingleMillimeters();
|
||||
bool timeout = sensor.timeoutOccurred();
|
||||
|
||||
TCA9548A_DisableAll();
|
||||
|
||||
if (distance == 65535 || timeout) return OUT_OF_RANGE_VALUE;
|
||||
|
||||
return distance;
|
||||
}
|
||||
|
||||
bool vl53l0xIsChannelActive(uint8_t channel) {
|
||||
return (channel < TCA9548A_CHANNELS && sensor_present[channel] && channel_enabled[channel]);
|
||||
}
|
||||
|
||||
int vl53l0xGetChannelCount() {
|
||||
int count = 0;
|
||||
for (uint8_t i = 0; i < TCA9548A_CHANNELS; i++) {
|
||||
if (sensor_present[i]) count++;
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
bool vl53l0xStartCalibration(uint8_t channel, uint16_t near_known_mm) {
|
||||
if (channel >= TCA9548A_CHANNELS || !sensor_present[channel]) {
|
||||
Serial.printf("ERROR: Channel %d not available\n", channel);
|
||||
return false;
|
||||
}
|
||||
|
||||
Serial.printf("Starting calibration for channel %d (near point: %d mm)\n",
|
||||
channel, near_known_mm);
|
||||
|
||||
uint32_t sum = 0;
|
||||
uint8_t valid = 0;
|
||||
|
||||
for (uint8_t i = 0; i < 20; i++) {
|
||||
uint16_t d = vl53l0xReadRawDistance(channel);
|
||||
if (d != OUT_OF_RANGE_VALUE) {
|
||||
sum += d;
|
||||
valid++;
|
||||
}
|
||||
delay(50);
|
||||
}
|
||||
|
||||
if (valid == 0) {
|
||||
Serial.println("ERROR: Failed to read near point");
|
||||
return false;
|
||||
}
|
||||
|
||||
calibration_near_raw = (uint16_t)(sum / valid);
|
||||
calibration_near_known = near_known_mm;
|
||||
calibration_channel = channel;
|
||||
calibration_in_progress = true;
|
||||
|
||||
Serial.printf("✓ Near point captured: raw=%d mm, actual=%d mm\n",
|
||||
calibration_near_raw, calibration_near_known);
|
||||
Serial.println("Now place object at FAR point and call vl53l0xFinishCalibration()");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool vl53l0xFinishCalibration(uint8_t channel, uint16_t far_known_mm) {
|
||||
if (!calibration_in_progress || channel != calibration_channel) {
|
||||
Serial.println("ERROR: Calibration not in progress or wrong channel");
|
||||
return false;
|
||||
}
|
||||
|
||||
Serial.printf("Finishing calibration for channel %d (far point: %d mm)\n",
|
||||
channel, far_known_mm);
|
||||
|
||||
uint32_t sum = 0;
|
||||
uint8_t valid = 0;
|
||||
|
||||
for (uint8_t i = 0; i < 20; i++) {
|
||||
uint16_t d = vl53l0xReadRawDistance(channel);
|
||||
if (d != OUT_OF_RANGE_VALUE) {
|
||||
sum += d;
|
||||
valid++;
|
||||
}
|
||||
delay(50);
|
||||
}
|
||||
|
||||
if (valid == 0) {
|
||||
Serial.println("ERROR: Failed to read far point");
|
||||
calibration_in_progress = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t far_raw = (uint16_t)(sum / valid);
|
||||
|
||||
Serial.printf("✓ Far point captured: raw=%d mm, actual=%d mm\n",
|
||||
far_raw, far_known_mm);
|
||||
|
||||
if (far_raw == calibration_near_raw) {
|
||||
Serial.println("ERROR: Raw values are identical");
|
||||
calibration_in_progress = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
float scale = (float)(far_known_mm - calibration_near_known) /
|
||||
(float)(far_raw - calibration_near_raw);
|
||||
float offset = (float)calibration_near_known -
|
||||
(float)calibration_near_raw * scale;
|
||||
|
||||
calibration[channel].valid = true;
|
||||
calibration[channel].near_raw = calibration_near_raw;
|
||||
calibration[channel].near_known = calibration_near_known;
|
||||
calibration[channel].far_raw = far_raw;
|
||||
calibration[channel].far_known = far_known_mm;
|
||||
calibration[channel].scale = scale;
|
||||
calibration[channel].offset = offset;
|
||||
|
||||
saveCalibration(channel);
|
||||
|
||||
Serial.printf("✓ Calibration complete: scale=%.5f, offset=%.2f\n", scale, offset);
|
||||
|
||||
calibration_in_progress = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void vl53l0xClearCalibration(uint8_t channel) {
|
||||
if (channel >= TCA9548A_CHANNELS) return;
|
||||
|
||||
calibration[channel].valid = false;
|
||||
calibration[channel].scale = 1.0f;
|
||||
calibration[channel].offset = 0.0f;
|
||||
saveCalibration(channel);
|
||||
|
||||
Serial.printf("✓ Channel %d calibration cleared\n", channel);
|
||||
}
|
||||
|
||||
bool vl53l0xIsCalibrated(uint8_t channel) {
|
||||
return (channel < TCA9548A_CHANNELS && calibration[channel].valid);
|
||||
}
|
||||
|
||||
CalibrationData vl53l0xGetCalibration(uint8_t channel) {
|
||||
if (channel >= TCA9548A_CHANNELS) {
|
||||
CalibrationData empty = {false, 0, 0, 0, 0, 1.0f, 0.0f};
|
||||
return empty;
|
||||
}
|
||||
return calibration[channel];
|
||||
}
|
||||
|
||||
void vl53l0xEnableChannel(uint8_t channel, bool enable) {
|
||||
if (channel >= TCA9548A_CHANNELS) return;
|
||||
|
||||
if (!sensor_present[channel]) {
|
||||
Serial.printf("ERROR: Channel %d not present\n", channel);
|
||||
return;
|
||||
}
|
||||
|
||||
channel_enabled[channel] = enable;
|
||||
Serial.printf("✓ Channel %d %s\n", channel, enable ? "enabled" : "disabled");
|
||||
}
|
||||
|
||||
bool vl53l0xIsChannelEnabled(uint8_t channel) {
|
||||
return (channel < TCA9548A_CHANNELS && channel_enabled[channel]);
|
||||
}
|
||||
|
||||
bool vl53l0xIsChannelPresent(uint8_t channel) {
|
||||
return (channel < TCA9548A_CHANNELS && sensor_present[channel]);
|
||||
}
|
||||
|
||||
void sensorTask(void *parameter) {
|
||||
vl53l0xInit();
|
||||
|
||||
for (;;) {
|
||||
vl53l0xLoop();
|
||||
vTaskDelay(pdMS_TO_TICKS(50));
|
||||
}
|
||||
}
|
||||
68
arduino_code/Test/src/vl53l0x_sensor.h
Normal file
68
arduino_code/Test/src/vl53l0x_sensor.h
Normal file
@@ -0,0 +1,68 @@
|
||||
#ifndef VL53L0X_SENSOR_H
|
||||
#define VL53L0X_SENSOR_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <VL53L0X.h>
|
||||
#include <Wire.h>
|
||||
#include <Preferences.h>
|
||||
|
||||
// Режимы измерения
|
||||
enum MeasurementMode {
|
||||
MODE_HIGH_ACCURACY = 0,
|
||||
MODE_PRECISION = 1,
|
||||
MODE_DEFAULT = 2,
|
||||
MODE_LONG_RANGE = 3,
|
||||
MODE_ULTRA_LONG = 4,
|
||||
MODE_COUNT = 5
|
||||
};
|
||||
|
||||
struct ModeProfile {
|
||||
const char* name;
|
||||
uint32_t timing_budget_us;
|
||||
uint8_t vcsel_prerange;
|
||||
uint8_t vcsel_final;
|
||||
float signal_rate_limit;
|
||||
uint16_t max_range_mm;
|
||||
uint8_t accuracy_mm;
|
||||
};
|
||||
|
||||
struct CalibrationData {
|
||||
bool valid;
|
||||
uint16_t near_raw;
|
||||
uint16_t near_known;
|
||||
uint16_t far_raw;
|
||||
uint16_t far_known;
|
||||
float scale;
|
||||
float offset;
|
||||
};
|
||||
|
||||
// Инициализация и цикл
|
||||
void vl53l0xInit();
|
||||
void vl53l0xLoop();
|
||||
|
||||
// Управление режимами
|
||||
bool vl53l0xSetMode(MeasurementMode mode);
|
||||
MeasurementMode vl53l0xGetMode();
|
||||
const ModeProfile* vl53l0xGetModeProfile();
|
||||
|
||||
// Чтение данных
|
||||
uint16_t vl53l0xReadDistance(uint8_t channel);
|
||||
uint16_t vl53l0xReadRawDistance(uint8_t channel);
|
||||
bool vl53l0xIsChannelActive(uint8_t channel);
|
||||
int vl53l0xGetChannelCount();
|
||||
|
||||
// Калибровка
|
||||
bool vl53l0xStartCalibration(uint8_t channel, uint16_t near_known_mm);
|
||||
bool vl53l0xFinishCalibration(uint8_t channel, uint16_t far_known_mm);
|
||||
void vl53l0xClearCalibration(uint8_t channel);
|
||||
bool vl53l0xIsCalibrated(uint8_t channel);
|
||||
CalibrationData vl53l0xGetCalibration(uint8_t channel);
|
||||
|
||||
// Управление каналами
|
||||
void vl53l0xEnableChannel(uint8_t channel, bool enable);
|
||||
bool vl53l0xIsChannelEnabled(uint8_t channel);
|
||||
bool vl53l0xIsChannelPresent(uint8_t channel);
|
||||
|
||||
void sensorTask(void *parameter);
|
||||
|
||||
#endif // VL53L0X_SENSOR_H
|
||||
11
arduino_code/Test/test/README
Normal file
11
arduino_code/Test/test/README
Normal file
@@ -0,0 +1,11 @@
|
||||
|
||||
This directory is intended for PlatformIO Test Runner and project tests.
|
||||
|
||||
Unit Testing is a software testing method by which individual units of
|
||||
source code, sets of one or more MCU program modules together with associated
|
||||
control data, usage procedures, and operating procedures, are tested to
|
||||
determine whether they are fit for use. Unit testing finds problems early
|
||||
in the development cycle.
|
||||
|
||||
More information about PlatformIO Unit Testing:
|
||||
- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html
|
||||
Reference in New Issue
Block a user