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:
Даня Архипов
2026-07-29 14:32:56 +00:00
parent 060971da75
commit 5cc27c6fc2
38 changed files with 9908 additions and 0 deletions

View 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

View 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

View 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));
}

View 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;
}

View 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

View 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));
}
}

View 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

View 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];
}

View 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

View 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));
}
}

View 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