Это оказалось очень удобно, интуитивно понятно и позволяет очень точно «рулить» роботом, что продемонстрировано на видео.
(По картинке — переход на видео на Рутубе)
Правда, продукт изменил форм-фактор и вместо небольшого модуля реализован на базе более солидного устройства, отладочной платы для AI-приложений.
Для знакомства самое простое, что можно сделать, это приобрести сам модуль управления и докупить отдельно робота CrowBOT от Elecrow, т.к. для него разработчик платы дополнил заводскую прошивку, включив блоки взаимодействия с отладочной платой.
Эта статья будет интересна тем, кто захочет использовать голосовое управление и управление наклоном платы в собственных проектах. Я объясню, как разработчик платы управления дополнил исходную прошивку робота, чтобы вы могли сделать то же самое для вашего устройства.
Как работает управление
Устройство реализовано на базе отладочной платы, на которой установлено ПО от разработчика. Плата подключается по Bluetooth к управляемому устройству и передаёт на него распознанные голосовые команды и углы наклона её самой. Пользователь держит этот «пульт» в руке и наклоном и голосовыми командами управляет роботом, или другими устройствами, которые самостоятельно адаптирует к работе с платой, в чём, надеюсь, поможет и эта статья.
На плате находится два микроконтроллера ESP32-S3 и ESP32-C3.
S3 уже прошит, при включении по умолчанию запускается программа управления роботом. Доступа к исходникам и возможности перепрошить S3 нет.
Взаимодействие и возможность что-то поменять на самом устройстве управления есть только на C3-части. Исходники прошивки для неё открытые, находятся тут github.com/MIR-LLC/AI-apps (для тех, кто захочет глубже разобраться или переделать механизм взаимодействия).
Первый способ управления: плата работает в режиме распознавания голосовых команд, но реализован только английский язык для фиксированного набора. Ниже — вольный перевод:
- ROBOT WAKE UP — выйти из спящего режима;
- ROBOT SLEEP — войти в спящий режим;
- MANUAL CONTROL — ручное управление;
- VOICE CONTROL — голосовое управление;
- GO RIGHT — направо;
- GO LEFT — налево;
- GO FORWARD — вперёд;
- GO BACK — назад;
- GO HOME — домой;
- SLOWER SPEED — медленнее;
- FASTER SPEED — быстрее;
- LIGHTS ON — включить огни;
- LIGHTS OFF — выключить огни;
- PLAY MUSIC — включить музыку.
(По картинке — переход на видео на Рутубе)
Второй способ управления — это управление изменением наклона платы в плоскости. При этом на управляемое устройство в режиме реального времени отправляются значения углов наклона по осям Х и Y, которые можно преобразовывать в команды управления движением.
Чтобы перейти в режим ручного управления нужно произнести команду MANUAL CONTROL, после чего экран платы сменит «картинку».
При этом голосовые команды также продолжают распознаваться и отправляться, и вы можете продолжать их обработку на управляемом устройстве.
(По картинке — переход на видео на Рутубе)
Готовая реализация для CrowBOT BOLT
Как я писал выше, разработчик платы уже реализовал возможность взаимодействия с платой «из коробки», правда для конкретного устройства — CrowBOT Bolt от Elecrow. Это учебный робот, который можно программировать в Arduino IDE, Letscode и MicroPython.
Заводская прошивка для робота от производителя есть на вики Elecrow, в ней уже реализована возможность управления с помощью ИК-пульта, который поставляется с роботом.
Нам же интересно сравнить исходную прошивку робота от производителя с новой прошивкой от разработчика платы управления, чтобы разобраться с кодом и взять его за основу для разработки программы для любых других устройств в собственных проектах.
Новая прошивка доступна здесь: https://github.com/MIR-LLC/CrowBot_program
Основные изменения в прошивке
- Подключение к DevBoard по Bluetooth
В исходной прошивке робота есть возможность работы с Bluetooth, т.к. робот может поставляться с Bluetooth-джойстиком для управления, но нам интересно, как реализовано подключение к нашей плате - Реакция на голосовые команды<
Обработка событий, которые приходят с платы и связаны с голосовым управлением - Управление роботом наклоном DevBoard
То же самое для управления наклоном платы - Звуковой сигнал при движении назад
Разработчик добавил полезные мелочи, которые сделали прошивку интереснее. Например, робот при любом движении задним ходом начинает издавать предупреждающий звуковой сигнал, как многие автомобили в современном мире. - Подсветка при поворотах (поворотники)
Также добавили подсветку в повороте, что делает использование CrowBOT более красочным и увлекательным.
Рассмотрим изменения подробнее
В самом начале программы подключаются заголовочные файлы библиотек, позволяющих организовать многозадачность:
13 │ #include <freertos/FreeRTOS.h> 14 │ #include <freertos/message_buffer.h> 15 │ #include <freertos/projdefs.h>Эти библиотеки обеспечивают создание и управление задачами, а также обмен сообщениями между ними. В дальнейшим это поможет в проигрывании звука и обработки команд от DevBoard.
Далее прописываются Bluetooth UUID DevBoard:
29 │ static BLEUUID serviceUUID("FFE0"); //Host service UUID 30 │ static BLEUUID charUUID("FFE1");Тем самым мы указываем устройство, к которому будет подключаться наш робот.
83 │ SemaphoreHandle_t xPlayMusicSemaphore; 84 │ SemaphoreHandle_t xDisconnectedSemaphore; 85 │ MessageBufferHandle_t xGrcCmdBuffer;Объявление семафоров и буфера сообщений для управления выполнением задач и синхронизацией между ними.
ⓘ
Что такое семафор?
Проще представить семафор как тарелку с одной конфеткой. Задачи могут проверить наличие конфетки, попробовать взять её и через время вернуть на место. Получается, это просто старый добрый bool, но работает он немного сложнее из-за многопоточности и связанных с этим проблем. Также стоит уточнить, что кроме бинарных семафоров (когда одна конфета), бывают и небинарные (несколько конфеток).
В нашем случае мы будем использовать семафоры для общения между задачами:
125 │ if (xSemaphoreTake(xPlayMusicSemaphore, portMAX_DELAY) == pdTRUE)Так, например, эта строчка обозначает «максимально долго ждать, пока на тарелке не появиться конфета, а как только она появиться, сразу её забрать».
258 │ xSemaphoreGive(xPlayMusicSemaphore);А эта строчка: «вернуть конфетку на место».
87 │ #define MAX_GRC_MSG_LEN 20Определение максимальной длины сообщения от DevBoard.
89 │ #define MOTOR_MIN_VALUE 30 90 │ #define MOTOR_MAX_VALUE 255Интервал абсолютных значений, подаваемых на моторы. Без этого ограничения двигатели робота просто гудят, но не работают. Добавление минимальной скорости поможет избавиться от шума на малой мощности.
92 │ template<class T> 93 │ constexpr const T& clamp(const T& v, const T& lo, const T& hi) 94 │ { 95 │ return std::less<T>{}(v, lo) ? lo : std::less<T>{}(hi, v) ? hi : v; 96 │ }Функция clamp позволяет ограничить диапазон изменения некоторого значения, задавая его нижний и верхний пределы.
98 │ void poly_transform(const float params[12], float x, float y, float *_x, float *_y) { 99 │ *_x = params[0] + params[1] * x + params[2] * y + 100 │ params[3] * powf(x, 2) + params[4] * x * y + 101 │ params[5] * powf(y, 2); 102 │ *_y = params[6 + 0] + params[6 + 1] * x + params[6 + 2] * y + 103 │ params[6 + 3] * powf(x, 2) + params[6 + 4] * x * y + 104 │ params[6 + 5] * powf(y, 2); 105 │ }Эта функция выполняет полиномиальное преобразование координат. Используется это в преобразовании углов наклона, получаемых от DevBoard, к мощностям, подаваемых на моторы. Стоит отметить, что это преобразование нелинейное. Как ранее говорилось, для двигателей рабочий интервал мощности [-255… 255], а получаемый угол — в интервале [-45 … 45].
Если взять 3 трёхмерных точки для каждого двигателя (крен, тангаж и ожидаемое значение мощности) и произвести интерполяцию, то можно получить формулу для такого преобразования. Например, для левого мотора берём точки:
(45, 45, 256)— DevBoard повёрнут вперёд и направо, то есть крутить левый мотор надо на максимальной мощности, чтобы заворачивать направо.
(-22.5, 45, 128)— DevBoard повёрнут вперёд и чуть левее, то есть надо ехать вперёд, но при этом заворачивать налево, поэтому ожидаем половину мощности на мотор.
(0, 0, 0)— наклона нет, движения тоже нет.
Если произвести билинейную интерполяцию получим для левого мотора формулу:
l = 1.3274074074074107x+3.792592592592593y+0.012641975308641975xyАналогично значения находятся и для правого двигателя.
107 │ void led_callback() //Callback function 108 │ { 109 │ FastLED.show(); 110 │ } 112 │ static bool moving_backwards = false; 113 │ void buzzer_callback() //Callback function 114 │ { 115 │ if (moving_backwards) { 116 │ ledcWriteTone(2, G4); //Buzzer 117 │ delay(150); 118 │ ledcWrite(2, 0); 119 │ } 120 │ }Процедуры led_callback и buzzer_callback используются для обновления состояния светодиодов и управления звуковым сигналом соответственно. Они будут вызываться через определённые интервалы.
122 │ void play_music_task(void*arg) 123 │ { 124 │ for (;;) { 125 │ if (xSemaphoreTake(xPlayMusicSemaphore, portMAX_DELAY) == pdTRUE) 126 │ { 127 │ if (!moving_backwards) { 128 │ for (int i = 0; i < Tone_length; i++) { //Buzzer 129 │ ledcWriteTone(2, Tone[i]); //Buzzer 130 │ delay(Time[i] * 1000); 131 │ } 132 │ ledcWrite(2, 0); //trun off Buzzer 133 │ } 134 │ } 135 │ } 136 │ }Задача, которая постоянно пытается захватить семафор и, если это ей удается, проигрывает музыку. Массив Tone содержит частоты, с которыми необходимо играть, а Time — время проигрывания той или иной ноты. Освобождение семафора происходит в главной задаче, тем самым давая возможность асинхронно выполнять голосовую команду PLAY MUSIC.
А вот собственно и главная задача:
138 │ void grc_cmd_task(void*arg) 139 │ { 140 │ const float d_speed = 0.5;Изменение коэффициента скорости (для голосовых команд FASTER SPEED и SLOWER SPEED):
141 │ float speed = 1;Сам коэффициент скорости (для команд GO FORWARD и GO BACK):
142 │ bool imu_control_mode = false;Находимся ли мы в режиме управления наклоном:
143 │ auto init_imu_control_state = [&imu_control_mode]() { 144 │ Serial.printf("Enable imu control mode\n"); 145 │ imu_control_mode = true; 146 │ ticker.attach_ms(300, led_callback); //lighting task 147 │ ticker1.attach_ms(900, buzzer_callback); //buzzer task 148 │ };При переходе в режим управления наклоном начинаем периодически вызывать led_callback и buzzer_callback:
149 │ auto release_imu_control_state = [&imu_control_mode, &moving_backwards]() { 150 │ Serial.printf("Disable imu control mode\n"); 151 │ imu_control_mode = false; 152 │ moving_backwards = false; 153 │ Motor(0, 0, 0, 0); 154 │ ledcWrite(2, 0); 155 │ fill_solid(leds, 4, CRGB::Black); 156 │ FastLED.show(); 157 │ ticker.detach(); 158 │ ticker1.detach(); 159 │ };При переходе в режим управления голосом выключаем двигатели и светодиод и останавливаем периодический вызов led_callback и buzzer_callback:
160 │ auto front_lights_cmd = [](bool front_lights_en) { 161 │ uint8_t val = 0; 162 │ if (front_lights_en) { 163 │ FastLED.setBrightness(255); //RGB lamp brightness range: 0-255 164 │ val = 255; 165 │ } 166 │ myRGBcolor6.r = val; 167 │ myRGBcolor6.g = val; 168 │ myRGBcolor6.b = val; 169 │ fill_solid(RGBleds, 6, myRGBcolor6); 170 │ FastLED.show(); 171 │ };Включаем (front_lights_en == true) или выключаем (front_lights_en == false) «фары»:
173 │ char msg[MAX_GRC_MSG_LEN]; Буфер сообщения:
174 │ for (;;) { 175 │ size_t length = xMessageBufferReceive(xGrcCmdBuffer, &msg, MAX_GRC_MSG_LEN, portMAX_DELAY); 176 │ std::string value(msg, length);Ожидаем сообщение из буфера. После получения переводим его в строку.
177 │ const std::string::size_type coords_offset = value.find("XY"); 178 │ const bool recv_imu_coords = coords_offset != std::string::npos; 179 │ if (recv_imu_coords && !imu_control_mode) { 180 │ init_imu_control_state(); 181 │ }Наличие XY в сообщении сигнализирует получение данных с датчика наклона. Если мы не в режиме управлением наклона и нам пришли XY, переходим в него.
183 │ //********************************GRC voice command****************************************** 184 │ if (!imu_control_mode) { 185 │ Serial.printf("Its characteristic value is: %s\n", value.c_str()); 186 │ if (value == "GO FORWARD") //forward 187 │ { 188 │ leds[2] = CRGB::Green; 189 │ leds[3] = CRGB::Green; 190 │ fill_solid(RGBleds, 6, myRGBcolor6); 191 │ FastLED.show(); 192 │ Motor(float(160) * speed, 0, float(160) * speed, 0); 193 │ delay(600); 194 │ Motor(0, 0, 0, 0); 195 │ fill_solid(leds, 4, CRGB::Black); 196 │ FastLED.show(); 197 │ } 198 │ 199 │ if (value == "GO BACK") //backward 200 │ { 201 │ leds[0] = CRGB::Red; 202 │ leds[1] = CRGB::Red; 203 │ fill_solid(RGBleds, 6, myRGBcolor6); 204 │ FastLED.show(); 205 │ Motor(0, float(160) * speed, 0, float(160) * speed); 206 │ delay(600); 207 │ Motor(0, 0, 0, 0); 208 │ fill_solid(leds, 4, CRGB::Black); 209 │ FastLED.show(); 210 │ } 211 │ 212 │ if (value == "GO RIGHT") //towards the right 213 │ { 214 │ leds[2] = CRGB::Green; 215 │ fill_solid(RGBleds, 6, myRGBcolor6); 216 │ FastLED.show(); 217 │ Motor(80, 0, 0, 80); 218 │ delay(350); 219 │ Motor(0, 0, 0, 0); 220 │ fill_solid(leds, 4, CRGB::Black); 221 │ FastLED.show(); 222 │ } 223 │ 224 │ if (value == "GO LEFT") //towards the left 225 │ { 226 │ leds[3] = CRGB::Green; 227 │ fill_solid(RGBleds, 6, myRGBcolor6); 228 │ FastLED.show(); 229 │ Motor(0, 80, 80, 0); 230 │ delay(350); 231 │ Motor(0, 0, 0, 0); 232 │ fill_solid(leds, 4, CRGB::Black); 233 │ FastLED.show(); 234 │ } 235 │ 236 │ if (value == "FASTER SPEED") // change speed 237 │ { 238 │ speed = std::min(1.5f, speed + d_speed); 239 │ } 240 │ 241 │ if (value == "SLOWER SPEED") // change speed 242 │ { 243 │ speed = std::max(0.5f, speed - d_speed); 244 │ } 245 │ 246 │ if (value == "LIGHTS ON") // enable lights 247 │ { 248 │ front_lights_cmd(true); 249 │ } 250 │ 251 │ if (value == "LIGHTS OFF") // disable lights 252 │ { 253 │ front_lights_cmd(false); 254 │ } 255 │ 256 │ if (value == "PLAY MUSIC") // toggle music 257 │ { 258 │ xSemaphoreGive(xPlayMusicSemaphore); 259 │ } 260 │ 261 │ if (value == "MANUAL CONTROL") // enable imu_control_mode 262 │ { 263 │ init_imu_control_state(); 264 │ }Выполняем команды в голосовом режиме:
265 │ } else { 266 │ if (value == "VOICE CONTROL") // disable imu_control_mode 267 │ { 268 │ release_imu_control_state(); 269 │ } 270 │ if (value == "LIGHTS ON") // enable lights 271 │ { 272 │ front_lights_cmd(true); 273 │ } 274 │ if (value == "LIGHTS OFF") // disable lights 275 │ { 276 │ front_lights_cmd(false); 277 │ } 278 │ if (value == "PLAY MUSIC") // toggle music 279 │ { 280 │ xSemaphoreGive(xPlayMusicSemaphore); 281 │ }А теперь обработка голосовых команд в режиме управления с помощью наклона.
Перейдем к обработке угла наклона:
282 │ //********************************GRC imucommand****************************************** 283 │ if (recv_imu_coords) { 284 │ int8_t ix, iy; 285 │ memcpy(&ix, &value.c_str()[coords_offset + 2], sizeof(int8_t)); 286 │ memcpy(&iy, &value.c_str()[coords_offset + 3], sizeof(int8_t));Получаем 2 байта из сообщения: угол по X и Y
287 │ 288 │ float x, y; 289 │ x = clamp(float(ix), -45.f, 45.f); 290 │ y = clamp(float(iy), -45.f, 45.f); Ограничиваем значения в интервале [-45… 45]
291 │ 292 │ static const float params[] = { 293 │ 1.689410597460024e-14, 1.3274074074074107, 3.792592592592593, 294 │ -1.0409086843526395e-17, 0.012641975308641975, -2.995326321410946e-18, 295 │ 3.215022330213294e-08, -1.3274074134624825, 3.7925925925925945, 296 │ 8.344758175431135e-18, -0.012641975308641976, -2.3814980193096928e-11}; 297 │ float l, r; 298 │ poly_transform(params, x, y, &l, &r);Собственно, преобразование углов наклона в значения двигателей.
300 │ if (l <= MOTOR_MIN_VALUE && l >= -MOTOR_MIN_VALUE) { 301 │ l = 0; 302 │ } 303 │ if (r <= MOTOR_MIN_VALUE && r >= -MOTOR_MIN_VALUE) { 304 │ r = 0; 305 │ } Если значения, подаваемые на моторы, достаточно малы, просто зануляем их. По сути это просто «шумодав» для двигателей, который был описан выше.
Тут возможно потребуется немного пояснения про «фары» робота:
Кроме RGB leds (дальнего света), есть ещё массив из 4-х фар:
— leds[0] — задний правый
— leds[1] — задний левый
— leds[2] — передний правый
— leds[3] — передний левый
307 │ fill_solid(leds, 4, CRGB::Black);По умолчанию фары выключены
308 │ int lf, lb, rf, rb; 309 │ if (r < 0.f) { 310 │ rf = 0; 311 │ rb = std::min(int(-r), MOTOR_MAX_VALUE); 312 │ } else { 313 │ rf = std::min(int(r), MOTOR_MAX_VALUE); 314 │ rb = 0; 315 │ myRGBcolor.r = 0; 316 │ myRGBcolor.g = rf; 317 │ myRGBcolor.b = 0; 318 │ leds[3] = myRGBcolor; 319 │ }Ограничиваем мощность моторов, если движение вперёд/влево, то включаем передний поворотник:
320 │ if (l < 0.f) { 321 │ lf = 0; 322 │ lb = std::min(int(-l), MOTOR_MAX_VALUE); 323 │ } else { 324 │ lf = std::min(int(l), MOTOR_MAX_VALUE); 325 │ lb = 0; 326 │ myRGBcolor.r = 0; 327 │ myRGBcolor.g = lf; 328 │ myRGBcolor.b = 0; 329 │ leds[2] = myRGBcolor; 330 │ }Аналогично для левого двигателя:
331 │ if (r < 0.f && l < 0.f) { 332 │ myRGBcolor.r = rb; 333 │ myRGBcolor.g = 0; 334 │ myRGBcolor.b = 0; 335 │ leds[0] = myRGBcolor; 336 │ myRGBcolor.r = lb; 337 │ myRGBcolor.g = 0; 338 │ myRGBcolor.b = 0; 339 │ leds[1] = myRGBcolor; 340 │ }При движении назад включаем сигнал заднего хода:
341 │ Motor(lf, lb, rf, rb); 342 │ if (l < -MOTOR_MIN_VALUE && r < -MOTOR_MIN_VALUE) { 343 │ moving_backwards = true; 344 │ } else { 345 │ moving_backwards = false; 346 │ } 347 │ } 348 │ } 349 │ } 350 │ }И наконец, сообщаем двигателям их мощность.
Эта процедура отвечает за приём сообщений от управляющего устройства:
352 │ static void NotifyCallback(BLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) { 353 │ xMessageBufferSend(xGrcCmdBuffer, pData, length, 0); 354 │ } Добавляет сообщение в буфер, который обрабатывается главной задачей.
На потери связи по Bluetooth:
366 │ // TODO: stop all activity on disconnect 367 │ Motor(0, 0, 0, 0); 368 │ xSemaphoreGive(xDisconnectedSemaphore);выключаем моторы и освобождаем семафор.
715 │ xPlayMusicSemaphore = xSemaphoreCreateBinary(); 716 │ xDisconnectedSemaphore = xSemaphoreCreateBinary(); 717 │ xGrcCmdBuffer = xMessageBufferCreate(MAX_GRC_MSG_LEN * 5); 718 │ xTaskCreate(play_music_task, "play_music_task", 1024, NULL, 1, NULL); 719 │ xTaskCreate(grc_cmd_task, "grc_cmd_task", 4 * 1024, NULL, 1, NULL);Создание семафоров, буферов и задач
На подключении к устройству инициализируем фары и моторы стандартными значениями:
764 │ FastLED.setBrightness(255); //RGB lamp brightness range: 0-255 765 │ fill_solid(leds, 4, CRGB::Black); 766 │ fill_solid(RGBleds, 6, CRGB::Black); 767 │ FastLED.show(); 768 │ Motor(0, 0, 0, 0); 769 │ ledcWrite(2, 0);При подключении пытаемся захватываем семафор отключения устройства:
780 │ xSemaphoreTake(xDisconnectedSemaphore, portMAX_DELAY);По сути ждём сигнала об отключения. А когда получим, возобновляем поиск устройств.
Заключение
Подведём итоги рассмотренных изменений в прошивке:
- Добавление многопоточности для параллельной обработки команд, независимо от их приёма, а также асинхронного проигрывания звуков
- Два режима управления с разными возможностями
- Полиномиальное преобразование угла поворота DevBoard в значения мощности моторов
Как видно из статьи, дело не обошлось парой правок фирменных исходников. Добавленный функционал сильно отличается от штатного, поэтому внесённых изменений довольно много. И если кто-то найдёт здесь что-то интересное для себя, то он может взять это в свои проекты.
В частности, если вам нужно голосовое управление, то вам достаточно получать сообщения от Bluetooth. Далее просто обрабатывать голосовые команды простым сравнением строк, остальное уже реализовано в приведённом выше коде. Ну, а для обработки угла наклона достаточно получить 2 и 3 байт исходного сообщения и преобразовать его в число со знаком. Это и будут углы поворота по X и Y.
Надеюсь, этот разбор поможет тем, кто захочет использовать плату управления для своих устройств.
И, разумеется, не надо бояться дорабатывать фирменные исходники. Как видим, глаза боятся, а руки делают. Всегда можно «доточить» функционал от скучных дядек, которые сидят в своих лабораториях под задачи, которые возникают у тех, кто работает «в полях» с реальным оборудованием.
ссылка на оригинал статьи https://habr.com/ru/articles/897420/
Добавить комментарий