17. Покращення дистанційного керування

 

Складніші задачі - цікавіші розв'язки

Останні кілька місяців ми були зайняті вдосконаленням системи віддаленого керування роботом. Перша версія, яку ми створили пів року тому, працює непогано, і має достатній запас функціональності, щоб покрити більшість  планованих нами задумів. І  RoboRemo з роллю пульта керування справляється просто фантастично.

Але кількох важливих речей таки бракувало:

• Можливості передавання відео з робота, щоб бачити обстановку навколо нього
• Можливості керувати роботом за допомогою джойстика або ігрового керма
• Великий екран, на якому можна розмістити детальну панель керування з купою перемикачів, індикаторів, потоком відлагоджувальних повідомлень і т.д.

Нічого готового до використання знайти не вдалося, тому ми взялися за написання власної програмки пульта керування.

Архітектуру залишили без змін. Вона в першу чергу диктується особливостями роботи Wifi шилда ESP13.

З точки зору Arduino - код робота, як і раніше, отримуватиме команди через послідовний UART порт, до якого підключено ESP13. Відповіді робот може записувати в той же ж порт у зворотному напрямку. З боку користувача може бути будь-який клієнтський застосунок (панель керування), який працює по telnet-подібному протоколу - тобто просто під'єднується до якогось TCP порта і вміє обмінюватися з ним байтами команд і відповідей.

Головну магію творить звичайно ж ESP13. Це є автономний модуль із власним мікроконтролером, багато в чому навіть потужнішим ніж наше Arduino. Саме він обслуговує бездротовий доступ, забезпечує зручний зв'язок з модулями Arduino а також несе на собі веб сервер куди можна під'єднатися і налаштувати роботу шилда в зручному і людяному вікні конфігурування. Зрештою, прочитайте попередній пост про першу версію віддаленого керування. Це допоможе зрозуміти як все працює та полегшить розбирання сьогоднішнього тексту.

Протокол

Командний протокол довелося трохи ускладнити. І з точки зору команд, які отримує робот, і з точки зору відповідей, які він може надсилати програмі дистанційного керування. Фактично, створення нового протоколу стало початковою точкою нашої роботи. Потім ми вже підганяли код робота і код панелі керування для виконання вимог цього протоколу.

Команди від панелі керування	Дія	Відповідь від робота (якщо все ОК)
MI	Режим - очікування	MI
MA	Режим - штучний інтелект	MA
MS	Режим - дії по сценарію	MS
MR	Режим - дистанційне керування	MR
W	Рух вперед	-
S	Рух назад	-
A	Поворот вліво	-
D	Поворот вправо	-
LF	Перемикання передніх вогнів	LF1 або LF0 1 - увімкнено 0 - вимкнуто
LR	Перемикання задніх вогнів	LR1 або LR0
LS	Перемикання додаткових вогнів	LS1 або LS0
R0	Переміряти відстані сенсорами (повертаючи також "голову")	На цю команду робот реагує серією відповідей: RLxx - відстань зліва. xx - це відстань в сантиметрах. При потребі число доповнюється нулями спереду, щоб завжди мати дві цифри. Відповідь "RL--" означає, що дані недоступні. RFxx - відстань спереду. Формат - такий самий. RRxx - відстань справа. Формат - такий самий. ROABCD - стан сенсорів перешкод (послідовність "1"-чок та "0"-ів), A - зліва попереду, B - зліва під гусеницею, C - справа під гусеницею, D - справа попереду
R1	Переміряти відстані сенсорами (не повертаючи "голову")	Відповідь - як і в попередньому випадку, але відстані справа і зліва ультразвуковим сенсором не виміряються
XAAABBB	Встановити швидкість двигунів у AAA (лівий) та BBB (правий). Значення швидкості може бути в межах від 000 до 511. 000 - це "повний назад", 511 - "повний вперед". 255 - стоп.	-
-	Якщо робот хоче щось повідомити оператору текстом, він повинен перед своїм повідомленням поставити тильду. Інакше в стандартному режимі повідомлення буде проігноровано.	~текст повідомлення. Нариклад: "~Вітаю, хазяїне!"

Всі команди і повідомлення повинні завершуватися символом завершення стрічки "\n".
Команди, що стосуються керування освітленням ми додали на запас на майбутнє. Поки що вони не використовуються, бо ми ще не придумали що з цим робити.

Код на стороні робота

Завдяки "магії" ESP8266 та шилда ESP13 для приймання команд і передавання відповідей на стороні Arduino не потрібно придумувати нічого особливого. Достатньо просто читати стрічки з послідовного порта і писати відповіді у нього. У випадку Arduino MEGA 2560 ми виділили окремий послідовний порт саме для цієї ролі. Платформа Arduino прямо з коробки підтримує такий обмін даними за допомогою стандартної бібліотеки Serial.

Писати відповіді взагалі просто. Виклик функції Serial.write(str)  відправить будь-яке повідомлення без проблем і затримок.

З читанням команд, які надходять до робота ззовні, все трохи складніше. Поки ми використовували однобайтні команди (як у першій версії) достатньо було час від часу питати в послідовного порта чи не прийшов бува якийсь байт, і якщо прийшов - читати його. Наш код виглядав десь так:

int remoteCommand = 0;

if (Serial3.available() > 0) {

    remoteCommand = Serial3.read();

    if (remoteCommand == 'M')

    <опрацювання команди> 

     ... 

 }

Для багатосимвольних команд таке вже працювати не буде.

На перший погляд - можна використати стандартну функцію Serial.readStringUntil('\n') яка тягне символи з послідовного порта аж поки не прийде символ закінчення повідомлення (ми домовилися, що це буде '\n').

Але ця функція також не годиться. Річ в тім, що на час, необхідний для витягування символів, виконання прошивки робота буде повністю призупинене. Послідовний порт сам по собі річ порівняно повільна, а якщо додати, що ми ще й очікуємо приходу символів з мережі - затримка може затягнутися на цілі секунди. І весь цей час робот буде без свідомості. Можливо навіть саме в цей момент він буде нестися на повній швидкості в стіну чи яму, без жодного шансу помітити перешкоду і оминути її. Якщо пам'ятаєте, нам навіть довелося спеціально переписувати прошивку робота, щоб уникати таких згубних провалів (див. пост Програмна архітектура і багатозадачність).

Єдиний спосіб безпечно вичитувати довгі стрічки з послідовного порта - це, як і раніше, отримувати символи повідомлення один за одним і накопичувати їх поступово в спеціальному буфері, не чекаючи поки прийде символ завершення команди. А як тільки символ завершення команди приходить - повідомляти модуль штучного інтелекту, що команда готова до опрацювання.

Для полегшення роботи з послідовним портом ми виділили весь відповідний код в окремий модуль - RobotConnector - та ізолювали його від решти прошивки.

RobotConnector слідує всім настановам свого батька (TaskInterface) тому може працювати як повноцінна задача в нашій системі багатозадачності. Завдяки цьому, він дає раду зі всіма своїми завданнями повністю авнотомно.

Модуль штучного інтелекту може звертатися до RobotConnector, використовуючи публічні методи:

• virtual bool commandAvailable() - для перевірки чи вже готова якась команда до опрацювання
• virtual char* getCommand() - для отримання тексту команди
• virtual void sendMessage(const char* message) - для відсилання відповіді

Тепер опрацювання команд в коді штучного інтелекту виглядає так:

if (robotConnector->commandAvailable()) {

        remoteCommand = robotConnector->getCommand();

        if (remoteCommand[0] == 'M')

        <опрацювання команди>

        ... 

Як тільки ми отримуємо повну команду у вигляді стрічки C/C++ (масиву символів) розібрати її і зреагувати відповідним чином - то вже справа техніки.

Код, який, власне, визбирує команду буква за буквою, розмістився в методі processTask() класу RobotConnector.

// processTask() викликається автоматично для всіх задач робота
// з головного циклу (див robot.ino)
void RobotConnector::processTask() {
 // якщо в буфері вже лежить команда, яка очікує на опрацювання
 // не робимо нічого, і чекаємо поки команду не заберуть
 if (!messageReady) {
  // прийшов наступний символ?
  while (Serial3.available() > 0) {

   // читаємо символ
   int incomingByte = Serial3.read();

   switch (incomingByte) {
   case -1:
    // чомусь не прочиталося - робити нема чого
    break;
   case COMMAND_TERM:    
    // це кінець повідомлення

    // нуль позначає кінець стрічки в C
    messageBuffer[messageSize++] = 0; 
    messageReady = true;
    break;
   default:
    // це просто собі один із символів повідомлення
    
    // не забуваємо перевіряти на переповнення буфера
    if (messageSize >= (MAX_MSG_SIZE - 1)) {
     // цього ніколи би не мало ставатися,
     // але якщо вже сталося - 
     //     забуваємо все що знали
     messageSize = 0;
    }
    
    messageBuffer[messageSize++] = incomingByte;
    break;
   }
  }
 }
}

Детальніше про проблему отримування довгих повідомлень з послідовного порту і способи її вирішення можна почитати в обговоренні на форумі Arduino або на форумі Ніка Гаммона.

Панель керування

Вимоги до системи та встановлення панелі керування

Панель керування було створено як незалежний проект RoboRemoteFPV, з окремим репозиторієм на GitHub. Ця програма є вільною і може використовуватися та поширюватися на умовах ліцензії GNU General Public License (GPL) як заповідає Фонд Вільного Програмного Забезпечення (FSF). Робіть з нашим кодом все, що заманеться, але якщо захочете поширювати його далі - не забирайте у ваших користувачів прав, які були надані вам самим на початку.

RoboRemoteFPV - є десктопною програмою. Це найкращий спосіб використати всі дюйми монітора максимально ефективно. Застосування технології Java з бібліотекою Swing дає можливість запускати нашу програму практично під будь-якою операційною системою, де встановлено свіжу версію Java. Хоча зараз вважається набагато модерновішим використання бібліотеки JavaFX, ми все ж обрали Swing, для безпроблемної роботи з відкритими версіями Java OpenJDK.

Ми тестували програму тільки під Windows, але ніщо не має заважати працювати їй і під GNU/Linux, і під MacOS. Особливих вимог до об'єму пам'яті нема. Якщо ваш комп'ютер може подужати Firefox чи Chrome - подужає і RoboRemoteFPV.

Для використання зовсім необов'язково знати Java і змінювати щось в коді. Cкачайте архів з дистрибутивом, розпакуйте його в зручне місце і запускайте roboremote.cmd (якщо під Windows).

Для показу відео з робота, RoboRemoteFPV використовує медіапрогравач VLC. Встановіть його на свій комп'ютер і скоріш за все наша програма зможе його автоматично знайти і під'єднати. Якщо у вас 64-бітна Java, постарайтеся поставити 64-бітну версію VLC.

Використання

Типовий сеанс використання панелі керування можна побачити на відео.

Приємним побічним ефектом використання VLC є те, що ви можете в RoboRemoteFPV вмикати будь-яке потокове відео чи аудіо. Наприклад ніхто не заважає вам підключитися до http://onair.lviv.fm:8000/lviv.fm і слухати радіо, прямо в процесі керування роботом. Правда відео з робота ви тоді не побачите.

З новим способом видавання команд роботу, керування стало набагато плавнішим. Особливо, якщо використовувати джойстик.

Структура коду програми

Скоріш за все - вам захочеться в програмі щось підлаштувати під себе. Ми намагалися зробити вихідні тексти максимально простими, тому навіть маючи знання із базового курсу Java можна змінити набір команд, переназвати їх на свій розсуд, або й додати якісь нові.

Вся програма розмістилася в шести модулях.

RoboRemote.java - це точка входу. Якщо викинути всі ритуальні фрагменти тексту - суть роботи цього модуля вкладається в три стрічки:

RoboCommandsModel.loadSettings(); // завантажуємо налаштування програми 
                                  // зі сховища

hidManager = new HIDManager();  // створюємо об'єкт, що керує джойстиками 
                                // та ігровими маніпуляторами

mainWindow = new MainWindow(); // створюємо головне вікно програми

RoboConnManager.java - це клас, який вміє під'єднуватися до робота, передавати йому команди і пильнувати за його відповідями.

Ключові функції:

• void connect(String address) - під'єднується до робота за вказаною адресою
• void disconnect() - від'єднується від робота
• void sendMessage(String message) - посилає роботу стрічку з довільним повідомленням
• void addMessageListener(RoboMessageListener listener) - реєструє слухача повідомлень від робота

З першими трьома методами все ніби просто. Вони виконують свою роботу зразу. Останній метод - це трюк в дусі Java і Swing. Оскільки ми не знаємо коли саме може прийти відповідь від робота, ми пропонуємо зовнішнім модулям зареєструвати свого слухача подій. І коли якесь повідомлення таки прийде - RoboConnManager негайно повідомить всіх слухачів про це.

Слухачем має бути клас, який реалізовує ось такий інтерфейс:

interface RoboMessageListener {
 void messageReceived(final String message);
 void disconnected();
}

"Реалізувати інтерфейс" означає мати в класі такі ж методи, які вказані в описі інтерфейсу. Коли від робота прийде якесь повідомлення, або коли робот відключиться від мережі - RoboConnManager буде знати який метод слухача викликати щоб повідомити про відповідну подію.

HIDManager.java - по своїй суті схожий на RoboConnManager. Але його задача вдвічі простіша. Він нічого не передає, а лише постійно перевіряє стан джойстиків та інших ігрових маніпуляторів. Як тільки щось в стані міняється (наприклад, користувач натиснув якусь кнопку, або відпустив педаль газу) - HIDManager негайно повідомляє своїм слухачам, що саме сталося.

Слухача можна зареєструвати викликавши метод addEventListener(HIDEventListener l). Причому слухач має реалізувати інтерфейс з єдиним методом, який запускатиметься якщо джойстик повідомить про якусь подію:

interface HIDEventListener {
 void actionPerformed(Event e);
}

Додатково HIDManager дає можливість прочитати список підключених ігрових  маніпуляторів, а також вибрати який з них вважати активним (методи getControllersList(), getSelectedController(), setSelectedController()).

Щоб не заморочуватися, ми не передбачаємо одночасне використання кількох ігрових контролерів. Але якщо комусь треба - додати таку можливість буде нескладно.

RoboCommandsModel.java - повністю закриває питання по обслуговуванню команд, які посилаються роботу. Заодно тут також забезпечено зберігання параметрів, які користувач вибрав у діалозі налаштувань (розкладка кнопок, IP адреси робота та відео потоків, активний джойстик, і т.д.)

В першу чергу вам мабуть захочеться поекспериментувати зі структурою CommandsList.

public static enum CommandsList {
  CMD_FORWARD("Forward", KeyEvent.VK_W, "W"),
  CMD_REVERSE("Reverse", KeyEvent.VK_S, "S"),
  CMD_LEFT("Left", KeyEvent.VK_A, "A"),
  CMD_RIGHT("Right", KeyEvent.VK_D, "D"),
  CMD_LIGHTS_FRONT("Lights Front", KeyEvent.VK_Z, "LF"),
  CMD_LIGHTS_REAR("Lights Rear", KeyEvent.VK_X, "LR"),
  CMD_LIGHTS_SIDES("Lights Sides", KeyEvent.VK_C, "LS"), 
  CMD_MODE_IDLE("Mode Idle", KeyEvent.VK_1, "MI"),
  CMD_MODE_AI("Mode AI", KeyEvent.VK_2, "MA"),
  CMD_MODE_SCENARIO("Mode Scenario", KeyEvent.VK_3, "MS"),
  CMD_MODE_RC("Mode RC", KeyEvent.VK_4, "MR"),
  CMD_RESCAN_DISTANCES("Rescan Distances", KeyEvent.VK_E, "R");
…

Це є основа всієї функціональності керування роботом. Для кожної команди тут задається:

• Назва команди, яка відображається в різних місцях панелі керування (наприклад "Lights Front")
• Типова кнопка клавіатури, яка активує команду (наприклад KeyEvent.VK_Z - що позначає кнопку "Z")
• Символи, які треба відправити роботу у випадку активування команди (для цієї ж команди - "LF")

MainWindow.java - найбільший і найстрашніший модуль. Його задача - створення головного вікна програми і прив'язування його компонентів до дій, які треба виконати для керування роботом.

createAndShowGUI() - це точка входу для коду створення всіх видимих елементів вікна. Цей метод викликається на етапі створення вікна і почергово запускає допоміжні методи, кожен з яких будує свою частину:

• initializeVideoPane() - відео панель зліва вгорі
• initializeMovementPane() - панель керування рухом і поворотами
• initializeLightsPane() - панель керування фарами і підствіткою
• initializeConnectPane() - панель керування під'єднанням команд і відео
• initializeLogPane() - панель відображення журналу повідомлень
• initializeDistancePane() - панель індикаторів відстані
• initializeModePane() - панель керування режимами роботи
• initializeButtonsPane() - панель з кнопками налаштування і виходу

initializeActions() - встановлює слухачів клавіатури, джойстика, повідомлень від робота. Також тут запускається таймер, який кожні 100 мілісекунд зчитує стан органів керування двигунами і посилає відповідну команду на двигуни робота. Той же ж таймер кожні 500 мілісекунд відправляє команду на швидке перечитування відстаней із сенсорів.

generateMotorsCommand() - це метод, який, знаючи відхилення осей джойстика, перераховує швидкість, з якою має обертатися кожен двигун робота.

processUserCommand(CommandRecord cRec) - це диспетчерський центр, куди стікаються всі події від користувача. Саме тут відбувається остаточне відображення подій в інтерфейсі і відсилання команд роботу.

processRobotMessage(String message) - містить код, який реагує на повідомлення від робота. Тут запалюються зелені і червоні лампочки індикаторів, та виводяться на екран відстані до перешкод.

Решта методів виконують косметично-допоміжні функції, тому можна на них увагу не сильно звертати.

Модуль PrefDialog.java відображає діалог налаштувань програми. Точка входу тут - метод showDialog().

Всі налаштування програми вичитуються з модуля RoboCommandsModel.  Всі зміни налаштувань, за згодою користувача, записуються назад в RoboCommandsModel (див. метод actionPerformed()).

Питання налаштування середовища розробки Java обговорювати не будемо. Ми використовуємо Eclipse. Не в останню чергу тому, що він підтримує також роботу і з C++, і через Sloeber навіть вміє заливати прошивки прямо на Arduino. Якщо маєте сумніви яке середовище розробки на Java вибрати - пошукайте на спеціалізованих сайтах. Порад і детальних інструкцій є достатньо.

Недоліки

RoboRemoteFPV має кілька недоліків, про які варто згадати:

1. Відео з камери показується на екрані з невеличкою затримкою. Керувати роботом по відео можна, але не настільки шустро, як би хотілося. За швидкість реакції відео відповідають конфігураційні параметри VLC вказані у файлі MainWindow.java
   
   

   String[] standardMediaOptions = {":network-caching=10",
                                    ":live-caching=10", 
                                    ":rtsp-caching=10"};

   Можливо, якщо підібрати кращі значення - затримка зменшиться. Але нам це поки не вдалося.
2. Вибір швидкості обертання двигунів робота залежно від ступеня відхилення ручки джойстика також можна покращувати. Зараз реакція робота є трохи "ватною". За це відповідає фрагмент коду у методі generateMotorsCommand(). Двигуни погано запускаються на малих потужностях. Також на звичайних джойстиках важко відхилити ручки до максимальних значень. По відчуттях здається так, ніби діапазон роботи двигунів штучно звужений. При плавному відхилення ручки джойстика робот спочатку не реагує ніяк, потім різко рушає, але до максимальної швидкості не розганяється. Якщо реакцію на відхилення ручки зробити нелінійною - поведінка робота здаватиметься більш природною. 
3. Теперішня версія програми не передбачає локалізації, тобто перекладу інтерфейсу на інші мови. Eclipse дозволяє перетворити всі стрічки у зручний для перекладання формат майже автоматично, і труднощів з тим нема ніяких. При нагоді, ми додамо як мінімум український переклад в наступних версіях.

Відео

З самого початку ми вирішили не передавати відео у вигляді радіосигналу, як це роблять більшість FPV систем. Не було бажання заморочуватися з тими всіма радіоприймачами і передавачами. Як варіант - вибрали встановлення на робота якоїсь недорогої IP камери.

Тема IP камер пророблена виробниками дуже добре. Легко можна знайти щось типу такого:

Таку невеличку камеру навіть можна встановити на коромисло сервомашинки і крутити нею навколо замість ультразвукового сенсора.

Головне - купити камеру, яка підтримує протокол RTSP, щоб можна було стандартними засобами показувати відео на пульті керування. Ну і також бажано переконатися, що камера не схожа на шпигунську, щоб доблесні захисники суверенітету України не сплутали вас з кимось іншим. Несхожими на шпигунські є камери, які в процесі запису блимають червоною лампочкою. Якщо раптом китайці лампочку припаяти забудуть - вам кримінальна стаття.

На щастя, нам не довелося купувати нічого такого. Завершення роботи над новою системою керування співпало із заміною моєї улюбленої мобілки Motorola Defy на новішу. Мотороли - штуки дуже живучі. А Defy - взагалі окрема історія. Спеціально посилений корпус, протиударне скло, водостійка конструкція (може плавати в глибоких водах), потужна батарея. Якось воно само собою вийшло, що ця мобілка увійшла в наш проект.

Для кріплення мобілки на роботі із пластикової заготовки (здогадайтеся з чого :-)) вирізали кронштейн і прикрутили його болтами на верхню площину робота.

Мобілка стає в кронштейн досить надійно. Пізніше ми його ще трохи підріжемо щоб мобілка опустилася нижче, а об'єктив дивився трохи вниз, попереду гусениць. Тоді на відео буде легше бачити перешкоди безпосередньо перед носом робота.

В ролі програми веб камери використовуємо IP WebCam. Комфортний режим відео - 480x360. Крім того ми увімкнули "Text overlay" щоб бачити стан заряду батареї мобілки прямо у відео на панелі керування. Корисною є й опція "Stream on device boot", яка дозволяє запускати передачу відео автоматично, зразу ж після старту.

Побічний ефект від використання мобілки в ролі відеокамери - на нашому роботі завівся окремий модуль, з власною батареєю живлення і досить потужними обчислювальними можливостями, особливо якщо порівнювати з Arduino. Також на ньому є акселерометр, компас, інші сенсори. І до цього модуля можна підключитися через Bluetooth. Гмм…. Скільки ж то ще всього цікавого можна з цим натворити!..

Підсумки і плани на майбутнє

Наш робот розвивається. З новою системою дистанційного керування - ним вже цікаво побавитися в реальних умовах. Схоже, що прийшов час зайнятися впорядкуванням під'єднань і схем - купа дротів і бредборд, який звисає збоку, не виглядають естетично і дуже заважають при доланні перешкод. Також не завадить реалізувати остаточну схему живлення і не тягати на спині Power Bank з USB кабелем.

Ну а в далекому майбутньому - висять перед нами мрії про справжній штучний інтелект, розпізнавання образів і звуків, осмислену навігацію навколишнім світом і інші круті речі. Поки ще ці мрії потребують визрівання і витримки.