Привет! На связи Михаил Дроздов, младший разработчик в Selectel. Заканчиваю рассказ о создании собственного робота. Предыдущая часть была целиком посвящена железу: выбору компонентов, особенностям проектирования и изготовления корпуса, распайке электроники.
В этой части говорим о разработке ПО. Микроконтроллер, видеокамера, датчики, двигатели — уже соединены, питание подключено. Осталось вдохнуть жизнь в электронный организм, чтобы все его компоненты пробудились и начали согласованно взаимодействовать.
Используйте навигацию, если не хотите читать весь текст
Программная архитектура
Ограниченные вычислительные ресурсы микроконтроллера не позволили реализовать всю многообразную логику на борту робота. Для выполнения сложных операций нужна машина помощнее — сервер.
Микроконтроллер берет на себя базовые задачи, например:
сбор данных,
управление моторами,
получение изображения с камеры,
передачу всей накопленной информации в центр обработки.
Сервер, в свою очередь, занимается сложными вычислениями — к примеру, обработкой изображений с помощью ИИ. Также он отвечает за управление роботом, отправляя команды.
Модель YOLO v8 не только классифицирует объекты, но и обнаруживает их — например, идентифицирует конкретных людей или предметы в кадре.
Размещение модели ИИ на микроконтроллере — задача невыполнимая из-за аппаратных ограничений. Кроме того, сервер может хранить примитивную карту местности, сформированную на основе информации от датчиков, что дополнительно улучшает навигацию.
Централизованный подход значительно упрощает масштабирование и упрощает архитектуру системы. В случае увеличения нагрузки можно добавить дополнительные вычислительные ресурсы: установить мощные GPU для обработки данных или подключить дополнительные серверы для распределения задач.
Клиентская часть
Встроенное ПО — это душа робота. Разработка велась в популярной у энтузиастов среде Arduino Framework, но с активным использованием возможностей операционной системы реального времени FreeRTOS, которая лежит в основе ESP-IDF. Еще одно немаловажное достоинство Arduino — множество готовых библиотек и примеров.
В классическом Arduino-скетче функции setup() и loop() отвечают за инициализацию периферии и непрерывный цикл обработки. В среде ESP32 Arduino Framework запускается как компонент ESP-IDF и по умолчанию выполняется в рамках одной FreeRTOS-задачи, что дает преимущества обеих платформ.
Документация ESP-IDF рекомендует создавать отдельные задачи на большие функции. Для их синхронизации требуются ресурсы, так как семафоры «не бесплатны». При таком подходе важно помнить о «стороже», который будет следить за выполнением каждой задачи. Если одна из них заберет на себя слишком много процессорного времени, «сторожу» придется вмешаться, что вызовет ошибку.
Асинхронный подход исключает подобные ситуации — внутри функций есть собственные таймеры, которые запускают выполнение кода. Таким образом, получаем две задачи, распределенные по ядрам.
Ядро 0 выполняет задачу
loopCore0— захватом изображения и сетевым взаимодействием. Кадры с камеры OV7670, отправляются по HTTP, а обмен данных происходит по WebSocket.Ядро 1 занято задачей
loopCore1— навигацией и управлением. Она опрашивает датчики, фильтрует данные, вычисляет ошибки и корректирует движение. Показания сохраняются в общий для обеих задач объект.
Общий объект робота содержит показания датчиков, действительные и заданные углы поворота, различные флаги состояния. Для синхронизации доступа к нему можно применить бинарные семафоры. Так гарантируется защита разделяемых ресурсов от состояния гонки и корректность операций чтения и записи в многозадачной среде.
Не забываем про датчик Холла, показания которого нужно постоянно обновлять. Чтобы не занимать процессорное время постоянным опросом, в прошивке предусмотрено аппаратное прерывание.
Аппаратное прерывание — механизм, который временно приостанавливает выполнение основного кода. В этот момент вызывается особая функция-обработчик, которая заранее привязывается к внешнему событию.
В нашем случае при возникновении сигнала на цифровом пине ввода-вывода, подключенном к датчику Холла, соответствующий блок кода как бы «вклинивается» в основную программу. Получается быстрая реакция на важные события, после чего управление возвращается.
Серверная часть
В основе серверного приложения лежит фреймворк FastAPI. Он обеспечивает прием входящих HTTP-запросов и установку WebSocket-сессий, выступая в роли центрального узла, через который робот обменивается командами и данными.
При первом соединении WebSocket-менеджер определяет тип подключения (оператор или робот) и сохраняет объект соединения в соответствующий список. Из каждой новой порции текстовых сообщений от робота парсер с помощью регулярных выражений извлекает телеметрию, например:
расстояния до препятствий спереди и сбоку,
напряжение батареи,
отметку времени.
угол поворота,
потребляемый ток.
Информация охраняется в локальной СУБД SQLite и сразу пересылается всем подключенным веб-клиентам.
Команды управления — запуск, приостановка, возобновление, различные движения по направлениям, а также полная остановка — транслируются в обе стороны. Так гарантируется синхронизация состояния между роботом и клиентским интерфейсом. При этом используется единственно допустимый набор значений в RobotStatus.
Поступающие от клиента WebSocket-сообщения также перенаправляются на робота. Прием изображений по REST-эндпоинту преобразует сырые байты в кадры PIL, подготавливает их для модели YOLO и выполняет детекцию человека. Далее, в зависимости от результата, генерируется соответствующая команда — пауза или возобновления работы. Готовый кадр кодируется в base64 и сразу же рассылается всем клиентам по WebSocket.
Общая схема обмена сообщениями, обработки телеметрии и взаимодействия с моделью детекции изображена на рисунке:
Selectel Tech Day — 8 октября
Разберем реальный опыт IT-команд, технический бэкстейдж и ML без спецэффектов. 15 стендов и интерактивных зон, доклады, мастер-классы, вечерняя программа и нетворкинг. Участие бесплатное: нужна только регистрация.
Основные циклы и общие переменные
Точка входа в программу — функция setup(). Внутри нее:
инициализируется последовательный порт,
активируется шина I2C,
настраиваются все сенсоры,
подключаются точка Wi-Fi,
устанавливается соединение с удаленным сервером посредством WebSocket,
инициализируется камера.
void setup() {
Serial.begin(USB_SPEED);
Wire.begin();
connectToWiFi();
connectToServer();
setupHallSensor();
setupIMU();
setupPowerSensor();
setupUltrasonicSensors();
#if CAMERA_ENABLED
setupCamera();
#endif
startMainLoops();
}В конце вызывается функция startMainLoops(), которая создает две независимые задачи FreeRTOS. Каждая из них закрепляется за своим ядром ESP32 — loopCore0 на Core 0 и loopCore1 на Core 1, после чего loop() остается пустым, и все управление переходит на эти два цикла:
void startMainLoops() {
xTaskCreatePinnedToCore(loopCore0, "lc0", 8192, NULL, 1, NULL, 0);
xTaskCreatePinnedToCore(loopCore1, "lc1", 8192, NULL, 1, NULL, 1);
}В теле задачи loopCore0 происходит сбор и отправка данных на сервер, а при включенной камере — захват кадра и отправка POST-запроса:
void loopCore0(void *pvParameters) {
while (true) {
stepLoop0();
pollAndSendData();
#if CAMERA_ENABLED
takeImageAndSendPostRequest();
#endif
vTaskDelay(pdMS_TO_TICKS(1));
}
vTaskDelete(NULL);
}Функция stepLoop0() измеряет время между итерациями и сохраняет значения в navo.dt_loop0, что позволяет синхронизировать обработку данных.
Переменные t_loop0, t_loop1 в классе Navo имеют формат double и хранят время в секундах. Такой подход позволяет избежать быстрого переполнения, характерного для целочисленных типов. Например, если использовать стандартный тип unsigned long с миллисекундами, переполнение наступит уже через 49 дней непрерывной работы, тогда как double в секундах гарантирует стабильную работу в течение нескольких лет, прежде чем потеряется точность на долях секунды.
Дополнительно реализована защита от переполнения и появления отрицательных значений, которые могут возникнуть при сбросе счетчика времени внутри микроконтроллера.
Вместо прямого использования сохраненного времени, система всегда оперирует его разницей. На каждой итерации цикла рассчитывается дельта между настоящим моментом и предыдущей итерацией. Полученное значение проверяется и, если оказывается некорректным, принудительно обнуляется. Затем результат сохраняется в dt_loop0 или dt_loop1.
Такой подход гарантирует, что все расчеты основываются только на интервалах, которые остаются корректными даже при случайных сбросах времени:
void stepLoop0() {
double now = micros() / 1000000.0;
navo.dt_loop0 = static_cast<float>(now - navo.t_loop0);
navo.t_loop0 = now;
if (!(navo.dt_loop0 > 0)) {
navo.dt_loop0 = 0;
}
}На Core 1 запускается второй цикл, отвечающий за навигацию и управление двигателями:
void loopCore1(void *pvParameters) {
while (true) {
stepLoop1();
updateYaw();
updateDistances();
autoMode();
updateVoltageAndCurrent();
vTaskDelay(pdMS_TO_TICKS(1));
}
vTaskDelete(NULL);
}Ключевые переменные хранятся в классе Navo:
distanceFrontиdistanceSide— отфильтрованные расстояния до стен,distanceHall— одометрический шаг по датчику Холла,yawиtargetYaw— текущий и целевой угол поворота,t_loop*иdt_loop*— метки и интервалы времени для каждого цикла,voltageиcurrent— параметры питания,autoModeиcorrection— состояние автоматики и направление корректировки,wheels— объект управления моторами.
class Navo {
public:
double distanceFront;
double distanceSide;
double distanceHall;
double yaw;
double targetYaw;
double t_loop0;
double t_loop1;
float dt_loop0;
float dt_loop1;
float voltage;
float current;
uint8_t autoMode = AutoMode::DISABLE;
uint8_t correction = Correction::NO;
uint8_t previousDirection = Direction::STOP;
uint8_t hallState = HallState::HALL_RESET;
WheelControl wheels = WheelControl();
Navo() {
wheels.attach(IN1, IN2, IN3, IN4);
wheels.stop(false);
}
inline String getStr() {
return "vol:" + String(voltage) + ",cur:" + String(current) +
",df:" + String(distanceFront) + ",ds:" + String(distanceSide) +
",ang:" + String(yaw) + ",t:" + String(t_loop1) +
",dh:" + String(distanceHall);
}
};
extern Navo navo;Благодаря такому разделению на два независимых цикла и единому хранилищу состояния в Navo, система способна одновременно обрабатывать сенсоры, отправлять данные и управлять движением. При этом минимизируются блокировки и задержки, свойственные однопоточному исполнению.
Работа с датчиками и управление двигателями
В модуле Sensors.cpp обработчик прерывания hallSensorISR() проверяет направление колес и время с момента последнего срабатывания — так отбрасываются дребезг и ложные триггеры. Затем при движении вперед или назад он прибавляет или вычитает шаг колеса — значение в миллиметрах хранит глобальная переменная:
void IRAM_ATTR hallSensorISR() {
static double tInterrupt = 0.0;
ESP_LOGI(mainLogTag, "Interrupt by Hall");
if (navo.wheels.direction == Direction::LEFT or
navo.wheels.direction == Direction::RIGHT) {
navo.previousDirection = navo.wheels.direction;
return;
}
if (navo.t_loop1 - tInterrupt <= 1.0) return;
tInterrupt = navo.t_loop1;
// The robot's going straight
if (navo.previousDirection == Direction::LEFT or
navo.previousDirection == Direction::RIGHT) {
navo.previousDirection = navo.wheels.direction;
return;
}
if (navo.wheels.direction == Direction::FORWARD)
navo.distanceHall += WHEEL_DISTANCE_MM;
else if (navo.wheels.direction == Direction::BACKWARD)
navo.distanceHall -= WHEEL_DISTANCE_MM;
navo.hallState = HallState::HALL_TRIGGERED;
}Используя библиотеку INA219_WE, функция updateVoltageAndCurrent() считывает напряжение и ток с датчика INA219. В свою очередь, updateDistances() каждые 0,1 с измеряет расстояние спереди и сбоку с помощью библиотеки HC-SR04. Полученные данные пропускаются через одномерный фильтр Калмана, также известный как фильтр «альфа‑бета», чтобы получить сглаженные и стабильные значения navo.DistanceFront и navo.DistanceSide.
Интервал 0,1 с выбран не случайно. Если постоянно опрашивать дальномеры, у процессора не останется времени на гироскоп, и его данные станут недостоверными. Чтобы предотвратить перерасход ресурсов, таймер distanceTime накапливает время на каждом шаге navo.dt_loop1 и сбрасывает его после обработки:
void updateDistances() {
static AlfaBetaFilter filterFront;
static AlfaBetaFilter filterSide;
static float distanceTime = 0.0;
distanceTime += navo.dt_loop1;
if (distanceTime <= 0.1f) return;
distanceTime = 0.0f;
double *distances = HCSR04.measureDistanceMm();
if (distances[0] != -1)
navo.distanceFront = filterFront.update(distances[0]);
else
navo.distanceFront = distances[0];
if (distances[1] != -1)
navo.distanceSide = filterSide.update(distances[1]);
else
navo.distanceSide = distances[1];
return;
}Расчет угла поворота navo.yaw выполняется в функции updateYaw() с помощью библиотеки MPU6050. Из исходного значения гироскопа вычитается текущее смещение для коррекции дрейфа. Затем результат интегрируется по времени dt_loop1 и нормируется, то есть приводится к диапазону от 0 до 360 градусов:
void updateYaw() {
static double yaw = 0.0;
static double tmp = 0.0;
gyroZ = imu.getRotationZ() - gyroZBias;
yaw += static_cast<double>(gyroZ) * navo.dt_loop1 / GYRO_SENSITIVITY;
tmp = fmod(yaw, 360.0);
navo.yaw = tmp < 0 ? tmp += 360.0 : tmp;
calibrateGyroOnce();
}Если робот останавливается более, чем на две секунды, функция calibrateGyroOnce() усредняет смещение гироскопа через простой фильтр низких частот, что позволяет компенсировать дрейф.
Код функции
calibrateGyroOnce()был позаимствован из проекта Flix GitHub, после чего адаптирован для использования.
Класс WheelControl управляет четырьмя GPIO-пинами двигателей и отвечает за начало движения и остановку, которые могут быть как плавными, так и мгновенными. После назначения выводов в методе attach() и их настройки в режим OUTPUT, метод smoothControl() циклически изменяет скважность ШИМ от нуля до заданного значения. Такой подход создает эффект постепенного разгона.
void WheelControl::smoothControl(uint8_t gpio1, uint8_t gpio2,
bool isStarting) {
const uint8_t steps = 10;
const uint8_t delay_time = 10;
uint8_t speed = direction;
int step = direction / steps;
if (!isStarting) step = -step;
for (int i = isStarting ? 0 : direction;
isStarting ? (i <= direction) : (i >= 0); i += step) {
analogWrite(gpio1, i);
analogWrite(gpio2, i);
delay(delay_time);
}
if (direction == Direction::FORWARD)
speed += 4;
else if (direction == Direction::BACKWARD) {
speed -= 4;
}
analogWrite(gpio1, isStarting ? speed : Direction::STOP);
analogWrite(gpio2, isStarting ? direction : Direction::STOP);
}На основе этого метода реализован функции forward(), backward(), left(), right() и stop(). Например, forward() при плавном старте обнуляет противоположную пару пинов, устанавливает direction = FORWARD и постепенно прибавляет мощность:
void WheelControl::forward(bool enableSmoothStart, bool enableSlowMode) {
analogWrite(in1, 0);
analogWrite(in3, 0);
direction = Direction::FORWARD;
if (enableSmoothStart)
smoothControl(in2, in4, true);
else {
uint8_t speed = enableSlowMode ? 60 : direction;
analogWrite(in2, speed + 4);
analogWrite(in4, speed);
}
}Итак, система сенсоров, фильтров и управления двигателями взаимодействует через общий объект navo. Два независимых цикла Core 0 и Core 1 микропроцессора ESP32 непрерывно считывают и обрабатывают данные, формируют координирующие сигналы и таким образом контролируют движение робота.
Управление камерой
Код для камеры взял из проекта ESP32CameraI2S на GitHub, однако очень многое пришлось переделать, чтобы все заработало как требовалось.
Инициализация и управление камерой OV7670 в проекте построена на наследовании от класса I2SCamera, отвечающего за прием параллельного потока пикселей по периферии I2S и синхронизацию по сигналам VSYNC, HREF и PCLK:
class OV7670 : public I2SCamera {
protected:
TwoWire *_wire;
uint8_t i2cAddress;
bool QQVGARGB565();
uint8_t writeRegister(uint8_t reg, uint8_t val);
bool QQVGA();
bool saturation();
bool frameControl(int hStart, int hStop, int vStart, int vStop);
bool writeRegisters(const RegisterValue regValues[], uint8_t count);
public:
bool reset();
bool ping();
OV7670(const uint8_t addr = 0x21) : _wire{&Wire}, i2cAddress{addr} {};
...При вызове OV7670::init() сначала настраивается внешний тактовый сигнал XCLK через функцию ClockEnable(pin, freq), а затем инициализируется прием по I2S и встраивается аппаратное прерывание по VSYNC:
if (!ClockEnable(XCLK, 10000000)) return false;
if (!I2SCamera::init(160, 120, VSYNC, HREF, XCLK, PCLK, D0, D1, D2, D3, D4,
D5, D6, D7))
return false;Затем система проверяет ответ камеры по шине I2C и настраивает ее для работы в режиме QVGA RGB565. Все эти действия объединены в методе QQVGARGB565(), который последовательно вызывает функции reset(), QQVGA(), frameControl() и saturation(). Каждая из них отвечает за свою серию записей в регистры камеры через интерфейс I2C:
bool OV7670::writeRegisters(const RegisterValue regValues[], uint8_t count) {
for (size_t i = 0; i < count; ++i) {
uint8_t i2cCode = writeRegister(regValues[i].reg, regValues[i].val);
if (i2cCode != 0) {
ESP_LOGE("Camera", "Error: %d, Reg: 0x%X, Val: 0x%X", i2cCode,
regValues[i].reg, regValues[i].val);
return false;
} else {
ESP_LOGD("Camera", "Successful: %d, Reg: 0x%X, Val: 0x%X",
i2cCode, regValues[i].reg, regValues[i].val);
}
}
return true;
}Базовый механизм приема кадров реализован в классе I2SCamera. При запуске настраиваются буферы DMA и подключаются прерывания I2S и VSYNC. После этого периферия I2S копирует необработанные данные в кольцевой буфер.
Технология DMA (англ. Direct Memory Access, прямой доступ к памяти) позволяет работать с данными напрямую в той области памяти, куда они были записаны, избегая лишнего копирования.
В завершение обработчик прерывания I2S извлекает данные из байтового потока и упаковывает их в итоговый массив frame[]:
void IRAM_ATTR I2SCamera::i2sInterrupt(void *arg) {
blocksReceived++;
unsigned char *buf = dmaBuffer[dmaBufferActive]->buffer;
if (blocksReceived == yres) {
framesReceived++;
stopSignal ? i2sStop() : void();
}
}Функция i2sInit() настраивает все выводы GPIO (VSYNC, HREF, PCLK и шину данных D0−D7) через матрицу сигналов ESP32. Она также включает параллельный режим I2S и конфигурирует делители тактового сигнала. Такая настройка обеспечивает прием пиксельных тактов на необходимой частоте:
pio_matrix_in(D0, I2S0I_DATA_IN0_IDX, false);
…
gpio_matrix_in(VSYNC, I2S0I_V_SYNC_IDX, true);Класс DMABuffer выделяет в динамической памяти два кольцевых буфера (по 4 байта на пиксель), а его метод next() создает односвязный список дескрипторов для прямого доступа к памяти.
Процесс получения изображения построен следующим образом. Сначала камера OV7670 настраивается через шину I2C на режим захвата 160×120 RGB565. Затем аппаратный интерфейс I2S принимает данные с камеры в фоновом режиме. При этом однократный вызов функции oneFrame() блокирует выполнение программы до тех пор, пока не будет получен полный кадр. Такой подход позволяет легко интегрировать захват изображения в основной рабочий цикл Core 0:
#if CAMERA_ENABLED
takeImageAndSendPostRequest();
#endifДля настройки камеры OV7670 используются ключевые регистры. Запись в них происходит в методах reset(), QQVGA(), frameControl() и saturation(). Ниже приведено их назначение.
REG_COM7 (0x12) — управляет глобальным сбросом и выбором цветового формата. Установка бита 7 (0b1000 0000) сбрасывает камеру, а значение 0b0000 0100 задает режим RGB.
REG_CLKRC (0x11) — настраивает делитель тактовой частоты XCLK. Установка бита 7 разрешает двойной внешний такт, что позволяет снизить нагрузку на внешний генератор.
REG_SCALING_XSC (0x70) и REG_SCALING_YSC (0x71) — задают коэффициенты масштабирования изображения по горизонтали и вертикали.
REG_SCALING_DCWCTR (0x72) — определяют параметры подавления цветового шума при снижении разрешения.
REG_SCALING_PCLK_DIV (0x73) — управляют частотой PCLK во время масштабирования.
REG_COM8 (0x13) — включает и настраивает автоматическую регулировку экспозиции, усиления и баланса белого AEC/AGC/AWB.
REG_COM11 (0x3B) — управляет режимами шумоподавления и автоматическими настройками для частоты сети (50/60 Гц) и ночного режима.
REG_COM15 (0x40) — определяет формат выходных данных. Бит 4 (0x10) переключает RGB565, а биты 6−7 настраивают диапазон уровня черного.
Регистры управления цветовой матрицей (0x4F−0x54, 0x58) — отвечают за насыщенность и баланс цветовых каналов.
Запись в эти регистры через шину I2C позволяет гибко настраивать режимы работы камеры OV7670 — от базовых параметров захвата QVGA до тонкой подстройки цветового баланса и частоты кадров. Основное преимущество такого подхода в том, что все настройки можно выполнить один раз при инициализации. Это позволяет не занимать шину I2C во время основной работы.
Обмен данными и взаимодействие с сервером
Для связи робота с сервером используются два канала: WebSocket и HTTP POST. Первый, реализованный с помощью библиотеки ArduinoWebsockets, служит для двунаправленного обмена командами и телеметрией в реальном времени. Второй, использующий встроенный Arduino HTTP Client, предназначен для отправки изображений с камеры.
При запуске функция connectToServer() создает объект WebsocketsClient и назначает для него две функции обратного вызова:
onEventsCallback— логирует события, такие как открытие и закрытие соединения, а также ответы на ping-запросы.onMessageCallback— обрабатывает текстовые команды от сервера. Эта функция изменяет состояние роботаnavo.autoModeили вызывает методыnavo.wheels. для управления двигателями:forward(),backward(),left(),right(),stop().
void onMessageCallback(websockets::WebsocketsMessage message) {
if (message.data() == "start") {
navo.autoMode = AutoMode::ENABLE;
} else if (message.data() == "stop") {
navo.autoMode = AutoMode::DISABLE;
} else if (message.data() == "suspend") {
...
}Телеметрию периодически отправляет функция pollAndSendData(), которая вызывается из основного цикла ядра 0. Пока WebSocket-клиент доступен, он опрашивается методом poll().
Если сработал датчик Холла или прошло больше секунды с последней отправки, система формирует строку с текущими значениями и отправляет ее на сервер. После передачи данных одометра переменная distanceHall и соответствующий флаг сбрасываются.
void pollAndSendData() {
static float sendTime = 0.0;
if (wsClient.available()) {
wsClient.poll();
sendTime += navo.dt_loop0;
bool sendNow = false;
if (navo.hallState == HallState::HALL_USED) {
sendNow = true;
} else if (sendTime >= 1.0f) {
sendNow = true;
}
if (sendNow) {
String data = navo.getStr();
wsClient.send(data);
if (navo.hallState == HallState::HALL_USED) {
navo.distanceHall = 0;
navo.hallState = HallState::HALL_RESET;
}
sendTime = 0.0f;
}
} else {
ESP_LOGW(mainLogTag, "WS client is not available");
}
}Во время работы камеры после создания очередного кадра устанавливается HTTP-соединение с заданным URL. Через него в теле POST-запроса отправляются данные из массива camera.frame длиной camera.frameBytes. После получения ответа система логирует его код и текст:
void takeImageAndSendPostRequest() {
if (navo.autoMode == AutoMode::DISABLE or navo.autoMode == AutoMode::MANUAL)
return;
camera.oneFrame();
client.begin(endpoint);
int httpResponseCode =
client.sendRequest("POST", camera.frame, camera.frameBytes);
if (httpResponseCode > 0) {
String payload = client.getString();
ESP_LOGI(mainLogTag, "HTTP: [%d] %s", httpResponseCode,
payload.c_str());
} else {
ESP_LOGW(mainLogTag, "HTTP-response error");
}
client.end();
}Автоматический режим
Автоматический режим управляется конечным автоматом, состояние которого хранится в переменной navo.autoMode. Этот режим состоит из трех этапов: установка начальной точки, движение по траектории «змейка» и возврат в режим ожидания.
Команды start, stop, suspend и resume, поступающие от WebSocket-сервера, изменяют состояние navo.autoMode на ENABLE, DISABLE, SUSPEND или ACTIVE соответственно. При переходе в состояние ENABLE запускается процедура инициализации автоматического движения — за нее отвечает функция setStartingPoint(). Она сохраняет текущий угол робота navo.yaw в переменной navo.targetYaw и задает первую цель — поворот на –90°.
С помощью метода rotate() робот поворачивается до тех пор, пока разница между текущим и целевым углом не станет меньше 1°. Начинается движение вперед, пока передний дальномер не обнаружит препятствие на расстоянии менее 110 мм. Тогда робот останавливается, задает новый целевой угол +90° и снова выполняет поворот. Только после второго успешного цикла rotate() начальная точка считается установленной. Система сбрасывает одометр, переводит autoMode в состояние ACTIVE, и робот начинает движение по основному алгоритму «змейка».
void setStartingPoint() {
static bool slowStartEnabled = false;
static uint8_t startPoint = StartPoint::START_POINT_NOT_SET;
if (startPoint == StartPoint::START_POINT_NOT_SET) {
ESP_LOGI(mainLogTag, "Auto mode enabled, set start point");
navo.targetYaw = navo.yaw;
angle = -90;
startPoint = StartPoint::START_POINT_ROTATING;
} else if (startPoint == StartPoint::START_POINT_ROTATING) {
if (!rotate()) return;
startPoint = angle < 0.0 ? StartPoint::START_POINT_SETTING
: StartPoint::START_POINT_SET;
} else if (startPoint == StartPoint::START_POINT_SETTING) {
if (!slowStartEnabled) {
navo.wheels.forward(false, true);
slowStartEnabled = true;
}
ESP_LOGD(mainLogTag, "Front distance: %f", navo.distanceFront);
if (navo.distanceFront > 110.0 or navo.distanceFront == -1.0) return;
navo.wheels.stop(false);
slowStartEnabled = false;
angle = 90;
startPoint = StartPoint::START_POINT_ROTATING;
} else if (startPoint == StartPoint::START_POINT_SET) {
navo.distanceHall = 0.0;
navo.hallState = HallState::HALL_RESET;
navo.wheels.forward(true, false);
navo.autoMode = AutoMode::ACTIVE;
ESP_LOGD(mainLogTag, "Initial target yaw: %f", navo.targetYaw);
startPoint = StartPoint::START_POINT_NOT_SET;
return;
}
return;
}В состоянии ACTIVE на каждом шаге цикла Core 1 выполняются три подзадачи.
Первая из них — коррекция по боковому датчику. Когда срабатывает датчик Холла HallState::HALL_TRIGGERED, система рассчитывает угол поправки, отталкиваясь от разницы между расстоянием до бокового препятствия и пройденным путем.
void sideDistanceCorrection() {
if (navo.hallState != HallState::HALL_TRIGGERED) return;
if (navo.correction == Correction::NO) {
double correctionAngle =
asin((navo.distanceSide - FIXED_SIDE_DISTANCE) /
navo.distanceHall) *
180.0 / M_PI;
ESP_LOGD(mainLogTag,
"correction angle: %f, side distance: %f, hall distance: %f",
correctionAngle, navo.distanceSide, navo.distanceHall);
navo.targetYaw = navo.targetYaw - correctionAngle;
}
navo.hallState = HallState::HALL_USED;
}Вторая подзадача — коррекция по углу. Если разница между текущим navo.yaw и целевым navo.targetYaw углами превышает 5°, система выбирает направление TO_LEFT или TO_RIGHT и вызывает команду корректировки через wheels.correction(). Движение прямо возобновляется, когда абсолютная разница между заданным и текущим углом становится меньше 0,5°:
void performYawCorrection() {
if (navo.wheels.direction != Direction::FORWARD) return;
double diff = normilizeDiff(navo.yaw, navo.targetYaw);
if (navo.correction == Correction::NO) {
if (fabs(diff) < 5.0) return;
ESP_LOGI(mainLogTag, "START CORRECTION, yaw: %f, target: %f, diff: %f",
navo.yaw, navo.targetYaw, diff);
if (diff > 0.0)
navo.correction = Correction::TO_LEFT;
else
navo.correction = Correction::TO_RIGHT;
} else if (navo.correction == Correction::TO_RIGHT) {
navo.wheels.correction(true);
navo.correction = Correction::IN_PROGRESS;
} else if (navo.correction == Correction::TO_LEFT) {
navo.wheels.correction(false);
navo.correction = Correction::IN_PROGRESS;
} else if (navo.correction == Correction::IN_PROGRESS) {
double currentDiff = fabs(normilizeDiff(navo.yaw, navo.targetYaw));
ESP_LOGD(mainLogTag, "yaw: %f, diff: %f", navo.yaw, currentDiff);
if (currentDiff <= 0.5) {
navo.wheels.forward(false, false);
navo.correction = Correction::NO;
ESP_LOGI(mainLogTag, "STOP CORRECTION");
}
}
}Третья подзадача — обнаружение углов и выполнение разворота. За это отвечает CornerDetector, который по трем последовательным измерениям с переднего и бокового дальномеров догадывается, что роботу пора повернуть.
В момент обнаружения угла робот останавливает движение и задает угол поворота: +90° для внешних углов и –90° для внутренних. Затем для выполнения маневра вызывается метод rotate(). После завершения поворота детектор сбрасывается, и движение возобновляется:
void detectCorner() {
CornerCheckState checkState =
cornerDetector.getState(navo.distanceFront, navo.distanceSide);
if (checkState == CornerCheckState::CONFIRMING_INNER) {
navo.wheels.stop(false);
angle = 90;
return;
} else if (checkState == CornerCheckState::CONFIRMING_OUTER) {
navo.wheels.stop(false);
angle = -90;
return;
} else if (checkState == CornerCheckState::UNCONFIRMED) {
navo.wheels.forward(true, false);
return;
} else if (checkState == CornerCheckState::IDLE)
return;
if (!rotate()) return;
cornerDetector.reset();
navo.wheels.forward(true, false);
}Функция autoMode() объединяет эти этапы и реагирует на команды из AUTO_ENABLE, AUTO_DISABLE и SUSPEND:
void autoMode() {
if (navo.autoMode == AutoMode::ENABLE) {
setStartingPoint();
} else if (navo.autoMode == AutoMode::DISABLE or
navo.autoMode == AutoMode::SUSPEND) {
navo.wheels.stop(true);
}
if (navo.autoMode == AutoMode::ACTIVE) {
sideDistanceCorrection();
performYawCorrection();
detectCorner();
}
}Серверная часть
Рассмотрим архитектуру сервера и протоколы взаимодействия с клиентом — прием телеметрии от робота и последующее хранение данных в базе. Ниже описаны алгоритмы постобработки, которые отвечают за фильтрацию шумов, определение углов и линейную аппроксимацию для построения карты помещения.
С помощью веб-интерфейса оператор может в реальном времени наблюдать визуализированную траекторию движения, стены, а также отслеживать уровень заряда батареи и управлять роботом с помощью кнопок.
Прием данных
Обмен телеметрией с роботом организован по WebSocket-соединению. Эту задачу выполняет класс ConnectionManager из модуля connection_manager.py. Его первая функция — установление и классификация подключения. При новом соединении метод connect() принимает WebSocket и определяет тип подключения: робот или оператор.
async def connect(self, websocket: WebSocket, type_: ConnectionType):
await websocket.accept()
if type_ == ConnectionType.robot:
robot = Robot(websocket=websocket)
self._robots.append(robot)
elif type_ == ConnectionType.client:
self._clients.append(websocket)Для каждого робота — их может быть и несколько — создается объект robot, который хранит текущий WebSocket и внутреннее состояние.
Вторая задача — разбор входящих сообщений. С каждой отправкой робот отправляет строку вида:
vol:7.2,cur:150.0,df:250.0,ds:100.0,ang:45.0,t:12.345,dh:30.0Метод parse_robot_message() выделяет из нее MAC адрес и телеметрию:
robot_index = _find_robot_index_by_websocket(self._robots, websocket)
if robot_index is None:
return
mac_start_index = data.find("mac:")
log_start_index = data.find("vol:")
if mac_start_index != -1:
self._robots[robot_index].mac_address = data[mac_start_index + 4 :]
if log_start_index != -1:
battery_percent = _get_battery_percent(data)
state = parse_robot_state(data)
self._db.add(self._robots[robot_index].mac_address, state)
await self.send_data_to_client(f"data:log:{data},bat:{battery_percent}")
if data.find("done") != -1:
self._robots[robot_index].update(RobotStatus.stopped)Сама функция parse_robot_state() использует регулярное выражение для извлечения значений в RobotState:
def parse_robot_state(data: str) -> RobotState:
pattern = re.compile(
r"vol:(?P<vol>-?\d+(?:\.\d+)?),"
r"cur:(?P<cur>-?\d+(?:\.\d+)?),"
r"df:(?P<df>-?\d+(?:\.\d+)?),"
r"ds:(?P<ds>-?\d+(?:\.\d+)?),"
r"ang:(?P<ang>-?\d+(?:\.\d+)?),"
r"t:(?P<t>-?\d+(?:\.\d+)?),"
r"dh:(?P<dh>-?\d+(?:\.\d+)?)"
)
match = pattern.match(data)
if not match:
raise ValueError("Invalid input format")
return RobotState(
timestamp=float(match.group("t")),
distance_front=float(match.group("df")),
distance_side=float(match.group("ds")),
distance_hall=float(match.group("dh")),
angle=float(match.group("ang")),
)Уровень заряда батареи вычисляется отдельно по методу getbattery_percent(), исходя из напряжения в поле vol:
def _get_battery_percent(log_data: str) -> float:
voltage = float(log_data[4 : log_data.find(",cur")])
min_voltage = 6.0
max_voltage = 8.4
percentage = max(
0, min(100, int(((voltage - min_voltage) / (max_voltage - min_voltage)) * 100))
)
return percentageТретья задача — рассылка клиентам данных и управляющих команд роботу. После парсинга сервер отправляет всем подключенным клиентам обновленную строку с телеметрией и процентом заряда:
async def send_data_to_client(self, data: str):
for client in self._clients:
await client.send_text(data)Когда робот присылает сообщение «done», его статус изменяется на RobotStatus.stopped. При отключении робот удаляется из списка robots, а всем клиентам отправляется сигнал для очистки данных. Метод send_message_to_robot() отправляет роботу текстовые команды по WebSocket и обновляет его внутренний статус. При этом система контролирует логику переходов между состояниями — например, не позволяет повторно запустить уже работающего робота.
Таким образом, ConnectionManager — это центральный компонент, который выполняет следующие функции:
принимает и идентифицирует подключения;
парсит и сохраняет телеметрию;
рассчитывает уровень заряда батареи;
транслирует данные всем клиентам;
передает команды управления обратно роботам.
Обработка информации
В проекте все сырые записи телеметрии робота сохраняются в базу данных с помощью класса RobotDatabase. При получении каждой новой строки данных, она добавляется в таблицу states:
def add(self, mac: str, state: RobotState):
with sqlite3.connect(self.db_name) as conn:
cursor = conn.cursor()
cursor.execute(
"""
INSERT INTO states (mac, timestamp, distance_front, distance_side, distance_hall, angle)
VALUES (?, ?, ?, ?, ?, ?)
""",
(
mac,
state.timestamp,
state.distance_front,
state.distance_side,
state.distance_hall,
state.angle,
),
)
conn.commit()Далее функция get_valid() выполняет фильтрацию, отбраковывая шумовые или пропущенные измерения. Общий путь, пройденный по одометру, накапливается в переменной total. Когда система обнаруживает стабильную группу точек, где distance_hall равно нулю, а показания фронтального и бокового дальномеров почти не меняются (с допуском tolerance = 1.0), первая точка в этой группе помечается как угол:
for i in range(1, len(states)):
if states[i].distance_hall == 0:
if states[i].distance_front > 300 or states[i].distance_side > 300:
continue
sequence_length = min(3, len(states) - i)
group = states[i : i + min(sequence_length, i)]
is_similiar = all(
abs(group[j].distance_front - group[j + 1].distance_front) <= tolerance
and abs(group[j].distance_side - group[j + 1].distance_side)
<= tolerance
for j in range(len(group) - 1)
)
if is_similiar:
corner_state = group[0]
corner_state.is_corner = True
corner_state.total = (
total
+ states_valid[-1].distance_front
- corner_state.distance_front
)
if not states_valid[-1].is_corner:
states_valid.append(corner_state)
i = i + sequence_length
continueПосле отбора валидных точек они передаются в process_states(), где на основе угла state.angle определяется направление движения:
def process_states(states: list["RobotState"]):
robot_cords: list[Coordinate] = []
walls: list[list[Coordinate]] = [[], []]
previous_direction = Direction.forward
offset = Coordinate()
corner_offset = 0.0
k = 0
for state in states:
direction = get_direction(state.angle)
if direction != previous_direction:
if (
previous_direction == Direction.forward and direction == Direction.left
) or (
previous_direction == Direction.backward
and direction == Direction.right
):
_, y = zip(*walls[k + 1])
y_offset = np.median(np.array(y))
sign = -1 if direction == Direction.left else 1
x_offset = walls[k][-1].x + sign * corner_offset
offset = Coordinate(x_offset, y_offset)
elif (
previous_direction == Direction.left and direction == Direction.backward
) or (
previous_direction == Direction.right and direction == Direction.forward
):
x, _ = zip(*walls[k + 1])
x_offset = np.median(np.array(x))
sign = -1 if direction == Direction.backward else 1
y_offset = walls[k][-1].y + sign * corner_offset
offset = Coordinate(x_offset, y_offset)
k += 1
walls.append([])
previous_direction = direction
if state.is_corner:
corner_offset = state.distance_side * np.cos(
np.radians(state.angle - direction.value)
)
if direction == Direction.forward:
pos, wall_side, wall_front = handle_forward(state, direction, offset)
elif direction == Direction.left:
pos, wall_side, wall_front = handle_left(state, direction, offset)
elif direction == Direction.backward:
pos, wall_side, wall_front = handle_backward(state, direction, offset)
else:
pos, wall_side, wall_front = handle_right(state, direction, offset)
walls[k].append(wall_side)
walls[k + 1].append(wall_front)
robot_cords.append(pos)
return robot_cords, wallsЗатем для каждой группы точек стены выполняется линейная аппроксимация:
def fit_line(points):
x, y = zip(*points)
if np.ptp(x) < 100:
return None, np.mean(x)
m, b = np.polyfit(x, y, 1)
return m, bТочки пересечения смежных прямых вычисляются функцией intersection(). Отладочная визуализация, пример которой представлен на сгенерированном изображении ниже, строится в функции plot_map(), где черными линиями рисуются отрезки стен, а синими точками — траектория робота:
for i, (pt_start, pt_end) in enumerate(wall_segments):
xs = [pt_start[0], pt_end[0]]
ys = [pt_start[1], pt_end[1]]
ax.plot(xs, ys, c="k", linewidth=2)
robot_x, robot_y = zip(*robot_cords)
ax.scatter(robot_x, robot_y, c="blue", label="Robot")
ax.legend()
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_title("Map")
plt.axis("equal")
plt.grid()
plt.show()
Заключение
Проект превратился из технической задачи в исследовательский марафон. Пришлось детально разбираться во всем: от проектирования механики в САПР и тонкостей 3D-печати до разработки многоуровневой архитектуры питания, от низкоуровневого программирования на C++ под FreeRTOS до создания полноценного клиент-серверного приложения с использованием ИИ.
Пожалуй, самым трудным вызовом стала система навигации. В любом реальном проекте приходится идти на компромиссы, и выбор ультразвуковых датчиков HC-SR04 стал одним из них. Вместо того, чтобы дать роботу нормальное «зрение», пришлось научить его «слышать» мир — ориентироваться в пространстве с помощью эхолокации, подобно летучей мыши.
Ограничения аппаратной части спровоцировали и серию программных решений. Неточные и «шумные» данные потребовали усложнения алгоритмов фильтрации, непрерывную коррекцию курса и управления состоянием. В итоге робот получился не самым зорким, и недостаток визуального восприятия компенсирует программной смекалкой. Должен признать, что именно эта борьба с несовершенством «железа» дала самый ценный опыт.
Напоминаю, что начало истории — в первой части.
Надеюсь, что этот проект — не финал, а лишь начало, MVP. Размышляю о более мощной платформе на базе Raspberry Pi, оснащенной LiDAR, точными лазерными дальномерами и камерой высокого разрешения. Опыт, полученный при создании «слышащего» робота пригодится для следующего шага — машины с настоящим, высокоточным зрением.
Надеюсь, моя история вдохновит и кого‑то из читателей на собственные эксперименты.