Принцип дії
Прийшов час нарешті навчити робота орієнтуватися на місцевості і відправити його у "вільне плавання" по кімнаті. Ще раніше ми поставили на робота кронштейн із ультразвуковим сенсором відстані HC-SR04, але до цього моменту він виконував виключно декоративну функцію.
Як саме працює такий сенсор відстані тут описувати не будемо - раджу почитати спеціалізовані статті Ultra-Sonic "Ping" Sensor (англ.) або Ультразвуковой датчик измерения расстояния HC-SR04 (рос.).
Суть проста - в якийсь момент часу сенсор випромінює ультразвуковий імпульс. Той імпульс біжить вперед, відбивається від перешкоди (якщо вона є) і повертається назад в приймач сенсора. Знаючи швидкість звуку в повітрі (~300 м/с) легко порахувати яку відстань пробіг імпульс від сенсора до перешкоди.
Фізики будуть ще розповідати, що швидкість звуку при різних температурах повітря може бути різною, також різні поверхні по різному відбивають звук, тому сенсор ніколи не буде особливо точним чи надійним, але ж ми не марсохода збираємо, і не автономний автомобіль. Найгірше, що може зробити наш робот - це вбити себе об стіну.
Також на різних форумах люди з досвідом застерігають початківців від купи інших недоліків сенсорів такого типу:
- Можуть конфліктувати між собою. То правда. Проблема вирішується опитуванням сенсорів строго по черзі, щоб сигнал одного не вводив в оману інших. Але це не працює, наприклад, у боях автономних роботів - сенсори одного робота можуть заважати роботі сенсорів іншого.
- Не помічають дрібних перешкод. То правда. І як виявилося - сенсори легко пропускають навіть досить великі конструкції типу ніжок столів.
- Нервують домашніх кішок і собак, бо ті ніби то чують ультразвуковий писк сенсора. Це неправда. Ми ретельно перевірили дію сенсора на нашу кицьку і не побачили нічого, крім щирого здивування і бажання попробувати на зуб погано закріплені провідники.
Для використання сенсора не потрібно заморочуватися зі всякими там бібліотеками. Достатньо у ваш код вписати функцію типу такої:
// повертаємо відстань від сенсора до перешкоди у сантиметрах float getDistance() { digitalWrite(triggerPin, LOW); delayMicroseconds(2); digitalWrite(triggerPin, HIGH); delayMicroseconds(10); digitalWrite(triggerPin, LOW); long duration = pulseIn(echoPin, HIGH); return duration /29 / 2; }
Роздивляємося на всі боки
У сусідніх схожих проектах ми підглянули цікаве рішення, коли ультразвуковий сенсор монтується на коромисло сервомашинки, що дозволяє роботу "дивитися по сторонах" і шукати перешкоди не тільки перед носом, а й правіше чи лівіше від курсу. Крутіння головою також моментально додає +500 до няшності робота, бо він тоді ну зовсім дуже стає схожим на легендарного Wall-E.
Ультразвуковий сенсор разом із кронштейном важать зовсім мало. Тому ми вибрали одну з найменших мікро-сервомашинок TowerPro SG90 Mini Gear Micro Servo 9g. Таке враження, що в шасі саме для такої цілі передбачено отвір, в який ця сервомашинка ідеально помістилася. А сам кронштейн прикрутився шурупами до симетричного коромисла, яке ішло в комплекті з машинкою.
 |  |
#include "Servo.h" // підключаємо бібліотеку ... Servo myservo; // створюємо змінну для керування машинкою ... myservo.attach(9); // підключаємо машинку до виходу #9 Arduino
myservo.write(90); // повертаємо машинку на позицію 90 градусів
Саме таку програмку варто виконати перед встановленням коромисла із кронштейном на машинку. Тоді буде легко виставити кронштейн рівно вперед по курсу.
Стандартна сервомашинка може обертатися в діапазоні між 0 та 180 градусами. Бажано все ж на дешевих машинках крайні позиції не вимагати - не факт, що машинка відкалібрована на заводі ідеально, можна даремно угробити шестерні. Залишайте запас 10-20 градусів по краях. На нашому роботі голова дивиться вперед в позиції "90 градусів" і може відхилятися вправо чи вліво на 45 градусів.
Зверніть увагу - в мініатюрних сервомашинках шестерні є дуже делікатними. Не намагайтеся їх провернути вручну навіть коли машинка вимкнена. А коли машинка увімкнена - вона буде всіма силами опиратися зміні позиції коромисла, бо так вона спроектована.
Ми свою сервомашинку трохи підпсували через баг в програмі. Замість того, щоб елегантно і плавно крутити головою зі сторони в сторону, ми засипали сервомашинку командами, які кидали її в протилежних напрямках. Поки я пробував усвідомити що стало причиною такої дикої поведінки, шестерні вилетіли з осей і заклинилися. І хоча їх потім вдалося розібрати і скласти назад - колишньої тиші вже не буде 😒
Кого цікавить внутрішня будова сервомашинки і принцип роботи - раджу суперове відео Electronic Basics #25: Servos and how to use them (англ.).
Вільне блукання
Алгоритм
Для проби сенсора відстані ми вирішили реалізувати простий сценарій блукання кімнатою:
- Робот їде вперед поки не побачить якусь перешкоду
- Помітивши перешкоду, робот зупиняється і пробує подивитися правіше і лівіше щоб визначити, де є більше простору для об'їзду.
- Робот повертає в напрямку, де більше простору. Якщо вибрати куди їхати неможливо - напрямок повороту вибирається випадково.
- Перехід до п. 1.
Потенційно такий алгоритм міг би дати повністю автономного робота, який може безконечно довго блукати кімнатами випадковим чином.
Робота із сенсором
Ми створили клас RobotDistanceSensor, який містить все необхідне для вимірювання відстаней ультразвуковим сенсором.
Виміряні відстані (в сантиметрах) можна прочитати викликаючи наступні методи:
- int8_t getFrontDistance();
- int8_t getLastFrontLeftDistance();
- int8_t getLastFrontRightDistance();
Назви методів наштовхують на думку, що вимірювання відстані попереду і по боках чимось відрізняються. Оскільки в більшості випадків сенсор робота дивиться вперед - відстань попереду ми можемо виміряти блискавично. А для того, щоб виміряти відстані правіше і лівіше - треба виконати цілий сценарій:
- Запустити сервомашинку на поворот правіше від курсу.
- Почекати поки вона повернеться.
- Виміряти відстань.
- Запустити сервомашинку на поворот лівіше від курсу.
- Почекати поки вона повернеться.
- Виміряти відстань.
- Запустити сервомашинку на поворот в центральне положення.
- Виміряти відстань попереду.
На відміну від простих прикладів з Інтернету, ми не можемо використовувати delay() в коді, очікуючи поки сервомашинка поверне голову в потрібному напрямку - будь-які оператори затримки вб'ють нашу багатозадачність (звуки перестануть звучати, світлодіоди мерехтіти і т.д.).
Тому модуль роботи із сенсором реалізовано як об'єкт-задачу, якому головна програма передає час від часу керування (викликаючи метод processTask()). Детальніше про реалізацію кооперативної багатозадачності ми писали раніше (див. 8. Програмна архітектура і багатозадачність).
Щоб отримати бокові відстані треба ініціювати їхнє вимірювання, викликом метода querySideDistances(). Виглядає він наступним чином:
void RobotDistanceSensor::querySideDistances() { // змінюємо стан на "міряємо відстань справа" dsState = dsMeasuringFR; // старі дані стають неактуальними - скоро будуть нові lastFLDistance = -1; lastFRDistance = -1; // якщо сенсор дивиться вперед - даємо 300 мілісекунд часу // на обертання вправо // якщо сенсор дивиться вліво - даємо вдвічі більше uint16_t servoDelay = (usServo.read()>F_POS) ? 2*SERVO_DELAY : SERVO_DELAY; // посилаємо команду сервомашинці на повертання в праве положення usServo.write(FR_POS); // тепер треба зачекати поки сервомашинка прокрутиться scheduleTimedTask(servoDelay); }
Після цього, модуль сенсора послідовно пройде через кілька станів вичікуючи кожен раз певний час поки прокрутиться машинка.
dsIdle, dsMeasuringFR, dsMeasuringFL, dsMeasuringFF - це назви станів, які використовуються в коді програми.
За проходження по цьому сценарію відповідає метод processTask(). Операційна система викликає його багато разів на секунду, даючи можливість виконати потрібні кроки як тільки наступить відповідний момент.
void RobotDistanceSensor::processTask() { // в бездіяльному стані - просто виходимо if(dsState != dsIdle) { // перевіряємо чи не наступив момент виконати якусь дію // згідно сценарію if (reachedDeadline()) { switch (dsState) { case dsIdle: // цей фрагмент більше ритуальний // середовище програмування нервується, // коли в операторі вибору перераховуються // не всі стани break; case dsMeasuringFR: // машинка завершила обертання в праву // позицію // виміряємо відстань і даємо команду // на поворот вліво lastFRDistance = getDistance(); usServo.write(FL_POS); // перемикаємо стан на // "міряємо відстань зліва" dsState = dsMeasuringFL; // чекаємо 300 мілісекунд // поки прокрутиться машинка scheduleTimedTask(SERVO_DELAY); break; case dsMeasuringFL: // машинка завершила обертання в ліву // позицію // виміряємо відстань і даємо команду // на повертання сенсора вперед lastFLDistance = getDistance(); usServo.write(F_POS); // перемикаємо стан на // "міряємо відстань попереду" dsState = dsMeasuringFF; // чекаємо 300 мілісекунд поки // прокрутиться машинка scheduleTimedTask(SERVO_DELAY); break; case dsMeasuringFF: // машинка завершила обертання // в позицію "вперед" // виміряємо відстань попереду - // і на цьому сценарій завершуємо lastFDistance = getDistance(); lastFDistanceTimeStamp = millis(); dsState = dsIdle; break; } } } }
Після завершення сценарію (через 1200 мс) - відстані готові до вичитування і використання для прийняття рішень штучним інтелектом.
Штучний інтелект
В модулі штучного інтелекту за всю розумову діяльність, як і раніше, відповідає метод processTask().
В режимі блукання кімнатою, робот може перебувати в одному з трьох станів:
- stateAI_GO - життя прекрасне, перешкод нема, повний вперед! Але час від часу робот міряє відстань до можливої перешкоди попереду. Якщо перешкоду помічено, і вона ближче, ніж 20 см - робот зупиняється, сенсору дається вказівка подивитися по боках, і робот перемикається в наступний стан.
- stateAI_QueryDistances - в цьому стані processTask() вмикається вже коли сенсор все переміряв. Штучний інтелект приймає рішення в якому напрямку здійснити поворот, щоб об'їхати перешкоду (повертати буде туди, де більше місця). На двигуни подається відповідна команда повороту, робот переходить в наступний стан.
- stateAI_Turning - робот завершив поворот. Повертаємося в стан stateAI_GO - а він вже запустить обидва двигуни на повний вперед.
А ось і фрагмент коду, який реалізовує це:
switch (currentAIState) { case stateAI_GO: { // читаємо відстань попереду (в сантиметрах) int8_t distance = robotDistanceSensor->getFrontDistance(); // є ситуації, коли відстань не прочиталася зразу // чекаємо тоді ще 300 мілісекунд і пробуємо знову if(distance < 0) { scheduleTimedTask(300); } else { // чи є перешкода попереду ближче, ніж 20 см? if(distance < MIN_DISTANCE) { // зупиняємо двигуни robotMotors->fullStop(); // пищимо здивованим голосом robotVoice->queueSound(sndQuestion); // просимо сенсор роздивитися по боках robotDistanceSensor->querySideDistances(); // перемикаємося в наступний стан currentAIState = stateAI_QueryDistances; // вичікуємо 3 секунди scheduleTimedTask(3000); } else { // перешкоди нема - повний вперед! robotMotors->driveForward(MOTOR_DRIVE_SPEED, 350); // наступного разу включити думалку // через 300 мілісекунд scheduleTimedTask(300); } } break; } case stateAI_QueryDistances: { // сенсор вже би мав передивитися відстані по боках // витягуємо їх у локальні змінні int8_t FLDistance = robotDistanceSensor->getLastFrontLeftDistance(); int8_t FRDistance = robotDistanceSensor->getLastFrontRightDistance(); if((FLDistance == -1) || (FRDistance ==-1)) { // може бути, що якась відстань не помірялася - // краще переміряти ще раз robotDistanceSensor->querySideDistances(); scheduleTimedTask(3000); } else { // відстані поміряні успішно if(FLDistance == FRDistance) { // якщо справа і зліва місця однаково - // вибираємо напрямок повороту випадково if(millis() % 2 == 0) { robotMotors->turnRight(MOTOR_TURN_SPEED, MOTOR_TURN_DURATION); } else { robotMotors->turnLeft(MOTOR_TURN_SPEED, MOTOR_TURN_DURATION); } } else // якщо вільного місця десь є більше - // повертаємо туди if(FLDistance < FRDistance) { robotMotors->turnRight(MOTOR_TURN_SPEED, MOTOR_TURN_DURATION); } else { robotMotors->turnLeft(MOTOR_TURN_SPEED, MOTOR_TURN_DURATION); } // перемикаємося в наступний стан currentAIState = stateAI_Turning; // змінюючи значення MOTOR_TURN_DURATION можна регулювати // кут повороту робота scheduleTimedTask(MOTOR_TURN_DURATION); } break; } case stateAI_Turning: // поворот пора припиняти // ми просто перемкнемося в початковий стан // і він пожене робота знову вперед currentAIState = stateAI_GO; break; } }
Повний код прошивки робота із доданим блоком блукання кімнатою: https://github.com/rmaryan/ardurobot/tree/ardurobot-1.2
Проблеми
В багатьох ситуаціях система блукання приміщенням поводила себе дуже адекватно. Робот акуратно оминає одиночні перешкоди:І навіть може просуватися між складнішими перешкодами:
Однак. кількість помилкових рішень виявилася надто великою, щоб можна було відпускати робота в автономний режим.
Основні проблеми:
- Сенсор розміщено надто високо над рівнем підлоги - робот просто не помічає перешкоди, якщо вони нижчі від рівня сенсора. Можна звісно ж опустити сенсор вниз, але тоді дуже постраждає зовнішня симпатичність робота, крім того визначати бокові відстані заважатимуть гусениці, які дуже видаються вперед.
- Поганий контроль за передніми габаритами. Ширина смуги сканування значно менша, ніж ширина робота. Постійно махати головою зі сторони в сторону - неестетично, шумно і непрактично.
- Поганий контроль за задніми габаритами. Повернути голову назад неможливо в принципі через обмеженість діапазону сервомашинки. Явно треба додавати якісь сенсори і ззаду.
- Мертва зона. Якщо робот умудрився під'їхати до перешкоди впритул - сенсор перестає бачити її. Якщо гусениці пробуксовують - робот не має жодного шансу зрозуміти, що він у щось вперся. Треба ставити сенсори дотику.
- Неточність і нестабільність роботи. Як виявилося - це досить серйозний фактор. Хоча фальшиві спрацювання можна якось виловлювати самою програмою робота, різну реакцію на різні матеріали мабуть подолати не вдасться ніяк.
- Пропускання невеликих перешкод (ніжок столів та стільців). Це також фундаментальна особливість сенсора, з якою боротися важко.
Немає коментарів:
Дописати коментар