Лабиринт

Теория

Логика движения робота в лабиринте следующая: едем вперёд, пока спереди расстояние больше 50 и справа меньше 100. Если справа расстояние больше 100, поворачиваем направо на 90 градусов. Если спереди расстояние меньше 50, и справа меньше 50, повернуть налево на 90 градусов.

Для упрощения управлением робота ниже представлена схема управления роботом на подобных колёсах:

Рисунок 2 - Схема управления роботом

Практика

Итак мы определились с движением робота в лабиринте, теперь нужно составить программу. Разделим её на несколько частей. Начнём с алгоритма выравнивая вдоль стены. Логика программы проста, если расстояние до датчика справа больше 20, уменьшаем скорость правых колес, если меньше 20, уменьшаем скорость левых колес. Для этого мы можем составить зависимость скорости моторов от показания датчика. Нам понадобятся следующие блоки:

 

Раздел “Модули”

• “Блок управления на 1 порту”  х1
• “Вращать 1 мотор со скоростью 50 об/мин”  х4

Раздел “Операторы”

• “+” х4
• “-” х4
• “*” х4

Раздел “Сенсоры”

• “ИК на 1 порту” х4

Начнём с составления уравнения для скорости моторов. Нам нужно составить следующее уравнение “10 + (ИК датчик -20) * 0.25”. В этом уравнении “10” - скорость моторов, “ИК датчик” - показание с ИК датчика расстояния, “-20” - расстояние до стенки, к которому мы стремимся, “0,25” - коэффициент для преобразования показания датчика в скорость. Составляем 4 таких уравнения для 4 моторов, не забываем указать порт на который установлен ИК датчика:

Теперь составим программу для отправления сигналов на моторы и добавим туда наши уравнения. Для этого нужно все блоки “Вращать мотор 1 со скоростью 50 об/мин” внутрь блока “Блок управления на 1 порту”, где также не забываем устанавливать порт к которому подключён блок стабилизатора, а также указать разные порты на блоках управления моторами. Перед тем как добавить наши уравнения вместо “50” в блоках управления моторов, нужно адаптировать их под каждый мотор. Начнём с того что 2 и 3 моторы установлены задом наперёд, следовательно их начальная скорость “10” должна быть отрицательной “-10”. При увеличении расстояния до стенки, правые колеса (3 и 4 моторы) должны замедлиться, а левые (1 и 2 моторы) ускориться, поэтому у 1 мотора коэффициент должен быть отрицательный “-0.25”, у 2 мотора положительный “0.25”, так как его скорость отрицательная и чтобы её уменьшить нужно прибавить положительное число. У 3 мотора коэффициент должен быть положительным “0.25” и у 4 мотора должен быть отрицательный коэффициент “-0.25”.

Теперь перейдём к программе поворота направо. Перед составлением программы нам нужно понять, как мы будем поворачивать. Для поворота направо нам нужно использовать поворот вокруг правого заднего угла робота (рис. 2). Для более менее точного поворота нужно рассчитать, на какое количество градусов нужно повернуть левые колеса, чтобы повернуть ровно на 90 градусов. Начнём с левого заднего колеса: ширина робота собранного по рисунку 1, примерно равна 15см. При повороте наше колесо пройдет по дуге равной 90 градусам, с радиусом 15 см. Давайте посчитаем длину пути которое должно пройти наше колесо для завершения поворота, для этого нужно посчитать длину дуги по формуле: L = 2 * π * R * a/360  , где π - математическая постоянная ~3.14, R - радиус окружности (в нашем случае это 15см), a - угол дуги. Получается L = 2*3,14*15*90/360 = 23,55 см. Теперь посчитаем какое расстояние пройдёт колесо за один оборот. Воспользуемся той же формулой, только вместо R будет 3см, так как радиус колеса равен 3см, а = 360 градусам, так как мы считаем полный оборот колеса. S = 2*3,14*3*360/360 = 18,84 см. Теперь осталось посчитать сколько градусов нужно провернуть колесо чтобы оно проехало 23.55 см. Считаем градусы по формуле a = L/S*360 = 450 градусов. Для второго колеса производи такие же расчёты, только заменяем 15 см на длину гипотенузы, зная что длина робота между задним и передним колесом примерно равна 14 см. R = √14²+15² = 20,52 см. L = 2*3,14*20,52*90/360 = 32,21 см. S остаётся таким же, так как радиус колес одинаковый. Считаем угол поворота для второго колеса a2 =  32,21/18,84*360 = 615 градусов.

Теперь перейдём к составлению программы. Нам нужно повернуть левые колеса на ранее рассчитанные градусы. Для этого нам понадобятся следующие блоки:

Раздел “Модули”

• “Блок управления на 1 порту”  х1
• “Вращать 1 мотор со скоростью 50 об/мин”  х2
• “Мотор 1 повернуть на 360 градусов со скоростью 50 об/мин”  х2
• “Ждать остановки 1 мотора на 1 порту” х2

Внутрь блока “Блок управления на 1 порту” добавляем блоки “Вращать 1 мотор со скоростью 50 об/мин” и “Мотор 1 повернуть на 360 градусов со скоростью 50 об/мин”. У блоков “Вращать 1 мотор со скоростью 50 об/мин”, меняем моторы на 3 и 4, и скорость на 0 об/мин, правая сторона согласно схеме на рисунке 2 должна стоять на месте. У блоков “Мотор 1 повернуть на 360 градусов со скоростью 50 об/мин” меняем номера моторов на 1 и 2. У 1 мотора ставим градус поворота 450 и скорость 10 об/мин, у 2 мотора ставим градус поворота -615 и скорость -10 об/мин. После блока “Блок управления на 1 порту”, ставим два блока “Ждать остановки 1 мотора на 1 порту”, указывая моторы которые мы поворачиваем на заданный угол, и порт на котором подключён блок стабилизатор.

Поворот налево реализуется так же как и поворот направо, только теперь мы двигаем правые колеса (3 и 4 моторы), а левые (1 и 2 моторы) стоят на месте. 4 мотор с градусом поворота 450 и скоростью 10, а 3 мотор с градусом поворота -615 и скоростью - 10.

Осталось собрать все подпрограммы в одну, составив условия для их активации. Нам понадобятся следующие блоки:

Раздел “События”

• “Когда контроллер запущен”  х1

Раздел “Управление”

• “Если то Иначе”  х1
• “Если то”  х2

Раздел “Операторы”

• “> 50” х2
• “< 50” х1

Раздел “Сенсоры”

• “ИК на 1 порту” х3

Блок “Всегда” присоединяем к блоку “Когда контроллер запущен”, внутрь блока “Всегда”, ставим блок “Если то Иначе”, с условием “ИК на 1 порту > 50”, этот ИК проверяет расстояние спереди. Внутрь блока “Если то Иначе”, после “Если то” ставим подпрограмму движения вперёд с выравниванием.

После программы выравнивания добавляем блок “Если то” с условием “ИК датчик > 100”, выбираем порт на котором присоединён правый ИК датчик, и внутрь “Если то” добавляем подпрограмму поворота направо.

Последнее что нужно сделать, после “Иначе”, добавить блок “Если то” с условием “ИК датчик < 50”, снова выбрав порт к которому подключён правый ИК датчик. Внутрь “Если то” добавляем подпрограмму поворота налево.  Таким образом, если наш робот упрётся двумя датчиками в стенки, он развернётся.

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