• Программирование делает вас лучше точно так же, как вам помогают развиваться изучение иностранного языка, математики или чтение книг.

LEGO — черепашка

Скачать статью в PDF

В основе этого проекта лежала простая идея унификации средств программного взаимодействия со сходными объектами.
В итоге — получилось нечто большее…

Да-да, вы не ошиблись, и здесь нет никакой опечатки: не привычная LOGO-черепашка, а именно LEGO-черепашка, вернее LEGO EV3 черепашка.

Этот проект возник по результатам школьных мытарств с Пайтоном. Бедные 8-классники за триместр с грехом пополам научились гонять виртуальную черепашку и, оставляя след, рисовать квадратики. Вряд ли стоит называть это «прогрессом». Однако, стоило отказаться от Пайтона в пользу MS Small Basic, дело пошло гораздо быстрее. Главное — появилось понимание. LOGO-черепашка в MS Small Basic быстро «научилась» ползать и рисовать разные картинки (далеко не только квадратики).

— А если мы захотим вместо черепашки запустить LEGO-робота, чтобы он бегал по таким же траекториям?

Класс впал в ступор…

— Думаете, это очень сложно?

— Конечно!

— А вот и нет! Мы же умеем управлять на экране компьютера виртуальным роботом — LOGO-черепашкой. Робот LEGO управляется не сложнее, если правильно подойти к решению задачи. Главное: у нас уже есть виртуальный прототип нашего робота, на примере которого мы сможем отработать все необходимые операции — наша LOGO-черепашка.

— И запомните: в программировании нет ничего сложного, если вы понимаете задачу и принципы её решения.

Программный код проекта мы специально убрали в раскрывающиеся блоки. Если вы хотите сами попробовать его написать, при этом, возможно, написать что-то немного другое — возможно, лучшее, пишите — не подглядывая. Но если вдруг у вас возникнет какая-то проблема, вы всегда сможете заглянуть в раскрывающийся блок и посмотреть, как это сделано у нас.  Для просмотра фрагментов кода — достаточно нажать на знак  +

Программный код
'Для начала "научим" черепашку ползать по экрану и рисовать квадраты.
Turtle.Show() ' показываем черепашку на экране
GraphicsWindow.PenWidth = 1 ' делаем толщину "следа" минимальной
Turtle.PenDown() ' опускаем перо для включения режима рисования

' Рисуем квадрат
Turtle.Move(200) ' идем вперед на 200 точек
Turtle.Turn(90) ' поворачиваем направо на 90 градусов
Turtle.Move(200) ' идем вперед на 200 точек
Turtle.Turn(90) ' поворачиваем направо на 90 градусов
Turtle.Move(200) ' идем вперед на 200 точек
Turtle.Turn(90) ' поворачиваем направо на 90 градусов
Turtle.Move(200)
1Попробуем понять, какие универсальные блоки команд мы используем для движения черепашки. Этих блоков не так много:
  • Инициализация черепашки (первые три команды — это будет подпрограмма RInit() )
  • Движение вперед и назад (Turtle.Move() — подпрограмма RMove() )
  • Поворот на любой угол (Turtle.Turn() — подпрограмма RTurn() )

Так как создавать в Small Basic мы можем только подпрограммы, то передавать значения в качестве параметров при вызове (как у функций) мы не можем, поэтому нам ещё понадобятся две переменные — параметры, управляющие движением:

  • Угол, на который должен повернуться робот (RAngle) «+» — направо, «—» — налево
  • Расстояние, которое робот должен пройти (RLength) «+» — вперед, «—» — назад
  • Скорость движения робота (RSpeed) от 1 до 10
2Итак, мы спроектировали свой, независимый от Small Basic программный интерфейс управления роботом и теперь мы можем заменить стандартные команды MS Small Basic написанными нами подпрограммами (в данном случае это будут так называемые «обертки»), которые просто позволят нам абстрагироваться от команд Small Basic. Теперь его нужно реализовать программно:
Программный код
' Пусть черепашка просто ползает и всегда оставляет след, чтобы был виден её путь.
' Тогда подпрограмма инициализации нашего робота-черепашки будет такой:
Sub RInit ' ("Robot Initialization" - инициализация робота)
    Turtle.Show() ' показываем черепашку на экране
    GraphicsWindow.PenWidth = 1 ' делаем толщину "следа" минимальной
    GraphicsWindow.PenColor = 0 ' задаем черный цвет следа
    Turtle.Speed = 5 ' для начала - задаем среднюю скорость движения черепашки - по-умолчанию
    RSpeed = 5 ' переменная управления скоростью для наших подпрограмм, которую мы сможем изменять
    Turtle.PenDown() ' опускаем перо для включения режима рисования
EndSub
'Робот,  в отличие от черепашки след оставлять не умеет, хотя мы можем воспользоваться, например, карандашом или маркером, закрепленным на корпусе робота так, чтобы при движении он оставлял след. Но главное - мы сможем отследить траекторию движение робота на экране с помощью черепашки, а затем воспроизвести то же самое движение в реальности - роботом.

' Теперь рассмотрим движение вперед и назад.
' Мы не можем передать в подпрограмму MS Small Basic параметы, как в функцию, и нам придется пользоваться общими переменными.
' Так, для задания расстояния движения мы создадим переменную RLength (Длина), в которую будем предварительно записывать значение расстояния, которое должен пройти робот.

' А для движения вперед и назад создадим подпрограмму RMove. Для того чтобы обеспечить движение назад достаточно указать отрицательное расстояние.
Sub RMove ' ("Robot Move" - робот движется)
    Turtle.Speed = RSpeed ' если значение переменной RSpeed изменилось - это надо учесть 
    Turtle.Move(RLength)
EndSub

' Теперь мы можем заставить нашего робота-черепашку поползать по экрану вперед и назад, используя уже нашу подпрограмму.
RInit() ' инициализируем робота
RLength = 200 ' задаем расстояние движения
RMove() ' робот движется вперед
RLength = -200 ' задаем расстояние движения
RMove() ' робот движется назад
' Все отлично работает!
3Движение робота (черепашки) по прямой с заданием скорости движения (при необходимости) у нас реализовано. Осталось реализовать повороты и проверить:
Программный код
Sub RInit ' инициализация робота
    Turtle.Show() ' показываем черепашку на экране
    GraphicsWindow.PenWidth = 1 ' делаем толщину "следа" минимальной
    Turtle.Speed = 5 ' задаем по умолчанию среднюю скорость движения черепашки
    RSpeed = 5 ' переменная управления скоростью для наших подпрограмм
    Turtle.PenDown() ' опускаем перо для включения режима рисования
EndSub

Sub RMove ' робот движется
    Turtle.Speed = RSpeed ' если значение переменной RSpeed изменилось - это надо учесть 
    Turtle.Move(RLength)
EndSub

' Теперь нам нужно "научить" робота поворачивать.
' Для этого напишем еще одну подпрограмму-обертку RTurn.
Sub RTurn ' ("Robot Turn" - робот поворачивает)
    Turtle.Turn(RAngle)
EndSub
' Теперь у нас готов полный интерфейс управления роботом. Для случая с черепашкой он крайне прост и представляет из себя набор из трёх простых подпрограмм-оберток.

' Давайте теперь попробуем управлять черепашкой с помощью нашего интерфейса.
' Нарисуем крест
RInit() ' инициализируем робота
RLength = 100 ' задаем расстояние движения
RMove() ' робот движется вперед
RLength = RLength * -1 ' меняем направление движения
RMove() ' робот движется назад
RAngle = 90 ' угол поворота - 90 градусов
RTurn() ' робот поворачивает
RMove() ' робот движется вперед
RLength = RLength * -1 ' меняем направление движения
RMove() ' робот движется назад
RTurn() ' робот поворачивает
RMove() ' робот движется вперед
RLength = RLength * -1 ' меняем направление движения
RMove() ' робот движется назад
RTurn() ' робот поворачивает
RMove() ' робот движется вперед
RLength = RLength * -1 ' меняем направление движения
RMove() ' робот движется назад
' Всё отлично работает. Используя эти команды, мы можем заставить двигаться черепашку как угодно и рисовать любые линии.
' Таким образом, мы обеспечили себе полную интерфейсную независимость от исходных функций MS Small Basic.
' То есть, при желании мы можем поменять содержимое наших функций, не меняя их сути и тем самым обеспечить изменение функционала при том,
' что текст программы, использующей эти функции, никак не изменится.
4Теперь давайте запишем кратко наши итоги и попробуем заставить наш робот-прототип (черепашку) выполнить какое-нибудь сложное движение.
Программный код
' Библиотека управления роботом (три подпрограммы-обертки)
Sub RInit ' ("Robot Initialization" - инициализация робота)
    Turtle.Show() ' показываем черепашку на экране
    GraphicsWindow.PenWidth = 1 ' делаем толщину "следа" минимальной
    Turtle.Speed = 5 'задаем по умолчанию среднюю скорость движения черепашки
    RSpeed = 5 ' переменная управления скоростью для наших подпрограмм
    Turtle.PenDown() ' опускаем перо для включения режима рисования
EndSub

Sub RMove ' ("Robot Move" - робот движется)
    Turtle.Speed = RSpeed ' если значение переменной RSpeed изменилось - это надо учесть 
    Turtle.Move(RLength)
EndSub

Sub RTurn ' ("Robot Turn" - робот поворачивает)
    Turtle.Speed = RSpeed ' если значение переменной Speed изменилось - это надо учесть 
    Turtle.Turn(RAngle)
EndSub
' ________________________________________________________________

' Сама программа управления роботом
RInit() ' инициализируем робота
RSpeed = 6
RLength = 20 ' задаем расстояние движения
RAngle = 15 ' угол поворота - 15 градусов
For i = 1 To 24 ' рисуем многоугольник
    RMove() ' робот движется вперед
    RTurn() ' поворот на 15 градусов
EndFor
RAngle = 90 ' угол поворота - 90 градусов
RTurn() ' поворот на 90 градусов направо
RLength = 200
RMove()' отползаем направо
For i = 1 To 4 ' рисуем квадрат
    RMove() ' робот движется вперед
    RTurn() ' поворот на 90 градусов направо
EndFor

Как видите, получилась довольно сложная траектория.

5Теперь нам нужно понять, чем же робот LEGO отличается от нашей экранной черепашки.
  • Ну, во-первых, у робота есть два мотора, которые обеспечивают его движение, и этими моторами надо как-то управлять.
  • Во-вторых, для определения угла поворота робота можно использовать гироскопический датчик.

Используя эти элементы мы получим такого же адекватно управляемого робота, как и наш прототип — черепашка. Для этой цели нам придется воспользоваться библиотекой функций EV3 Basic.

И если мы захотим, чтобы наш LEGO-робот прошел по такой же траектории, нам нужно будет только переписать наши три функции управления так, чтобы они управляли не экранной черепашкой, а реальным роботом. И всё! Саму программу движения это никак не затронет. То есть, если у нас уже есть какая-то сложная программа управления нашей черепашкой, то после замены содержимого наших функций мы получим ту же самую программу, но управляющую уже LEGO-роботом. Теперь мы можем сначала отработать траекторию движения робота на экране, а потом воспроизвести её в реальности.

6Для того чтобы управлять роботом LEGO надо сначала его спроектировать и построить. Нам понадобится центральный блок EV3, 2 больших мотора, гироскопический датчик, провода, конструктивные элементы из набора LEGO и резиновые гусеницы.

Для ходовой части мы взяли две рейки по 13 отверстий, закрепили на них с помощью осей 3 колеса, добавили конструктивную поперечину, установили резиновые гусеницы, получилась вот такая конструкция:

Для нашего робота понадобится два таких гусеничных блока.

В качестве несущей рамы и ходовой части мы соединили два больших мотора вот так:

Из штифтов и реек мы собрали пару инструктивных элементов, обеспечивающих жесткость всей конструкции и позволяющих закрепить на роботе сам блок EV3:

И закрепили их на нашей раме:

Осталось только установить на место гусеницы:

Наша LEGO-«черепашка» почти готова. Теперь нужно закрепить на ней блок EV3 и подключить к нему моторы с помощью проводов (к портам A и B).

После соединения моторов необходимо установить и подключить (к порту 1 блока EV3) гироскопический датчик. Используя его, мы сможем точно определять углы поворота нашего робота.

Робот полностью собран. Получилась устойчивая, маневренная и небольшая машина — настоящая «черепашка». Размеры робота 15 см Х 15 см и в высоту тоже — чуть меньше 15 см. Преимущество именно этой конструкции, в частности, в том, что у неё очень легко отделяется блок EV3, что позволяет быстро зарядить его, заменить батарейки или вообще переставить на другого робота. В принципе, конструкция робота может быть любой — это зависит от индивидуальной фантазии и инженерных идей.

7Для управления моторами мы будем использовать команды, которые не требуют последующей команды для остановки моторов — команды, обеспечивающие вращение моторов на заданный угол. При этом для движения робота вперед или назад будем использовать вращение моторов в одном направлении, а для поворота робота на месте — в противоположных.
Программный код
' Библиотека управления LEGO-роботом
Sub RInit ' инициализация робота
    RSpeed = 5 ' задаем скорость движения (если скорость не задана напрямую, по умолчанию она будет 50% от максимальной)
    MotoSpeed = RSpeed * 10 ' задаем скорость вращения моторов, исходя из значения переменной RSpeed
EndSub

Sub RMove ' движение робота
    If RLength < 0 Then ' определяем скорость и направление движения
        MotoSpeed = RSpeed * 10 * -1 '  если нужно ехать назад
    Else
        MotoSpeed = RSpeed * 10 ' если нужно ехать вперед
    EndIf
    MotoAngle = RLength' определяем угол вращения исходя из расстояния
    ' (так как изначально мы не знаем, сколько проезжает робот при повороте моторов на 1 градус, будем пока считать, что эти величины равны
    '  после эксперимента с замером проходимого расстояния мы сможем определить коэффициент, который их связывает для данной модели робота)
    Motor.MoveSync("AB", MotoSpeed, MotoSpeed, MotoAngle, "True") ' запуск синхронно обоих моторов (порты А и В) с одинаковыми скоростями для поворота на заданный угол с последующим торможением
EndSub
8Единственное, что потребует экспериментального определения — это проходимое нашим роботом расстояние в зависимости от угла поворота моторов. Эта величина напрямую зависит от конструкции ходовой части. Запустим нашего робота вперед на расстояние в 1000 условных единиц. Пока это будет поворот моторов на 1000о (мы пока не знаем, сколько при этом проедет робот).

Расстояние, пройденное роботом, удобнее всего измерять линейкой или рулеткой с миллиметровыми делениями, привязываясь к какой-то выбранной заранее точке на роботе. В качестве такой точки можно воспользоваться каким-нибудь внешним углом, конструкции или дополнительным штифтом, установленным так, чтобы он был перпендикулярен направлению движения и выступал наружу.

Программный код
' Библиотека управления LEGO-роботом
Sub RInit ' инициализация робота
    RSpeed = 5 ' задаем скорость движения (если скорость не задана напрямую, по умолчанию она будет 50% от максимальной)
    dir = 1 ' переменная, "исправляющая" значение угла поворота
    MotoSpeed = RSpeed * 10 ' задаем скорость вращения моторов, исходя из значения переменной RSpeed
EndSub

Sub RMove ' движение робота
    If RLength < 0 Then ' определяем скорость и направление движения
        MotoSpeed = RSpeed * 10 * -1 ' если нужно ехать назад
        dir = -1 ' чтобы значение угла было положительным
    Else
        MotoSpeed = RSpeed * 10 ' если нужно ехать вперед
        dir = 1
    EndIf
    MotoAngle = RLength * dir ' определяем угол вращения исходя из расстояния (при отрицательном расстоянии угол всё равно будет положительным)
    ' (так как изначально мы не знаем, сколько проезжает робот при повороте моторов на 1 градус, будем пока считать, что эти величины равны
    '  после эксперимента с замером проходимого расстояния мы сможем определить коэффициент, который их связывает для данной модели робота)
    Motor.MoveSync("AB", MotoSpeed, MotoSpeed, MotoAngle, "True") ' запуск синхронно обоих моторов (порты А и В) с одинаковыми скоростями для поворота на заданный угол с последующим торможением
EndSub
'_________________________________________________________

RInit()
RLength = 1000
RMove()
9Мы провели серию экспериментов, задавая разные значения расстояния для перемещения, и получили, что для прохождения одного сантиметра роботу нужно повернуть моторы на угол 36.63О. Следует иметь в виду, что датчики вращения моторов не безупречны, поэтому не стоит обольщаться насчет идеально точного перемещения робота: ошибка может составлять до 5 мм.

Для удобства примем следующий масштаб: один пиксель перемещения черепашки на экране равен одному сантиметру перемещения робота. Точнее проконтролировать перемещение такого робота — просто невозможно (ошибка — до 5 мм), поэтому такой масштаб — оптимален.

Кроме того, используя этот масштаб, мы легко можем запрограммировать робота на движение по помещению или лабиринту — как на экране, так и в помещении.

Итак, наша библиотека для робота приняла следующий вид (не хватает пока функции поворота робота):

Программный код
' Библиотека управления LEGO-роботом
Sub RInit ' инициализация робота
    RSpeed = 5 ' задаем скорость движения (если скорость не задана напрямую, по умолчанию она будет 50% от максимальной)
    dir = 1 ' переменная, "исправляющая" значение угла поворота
    MotoSpeed = RSpeed * 10 ' задаем скорость вращения моторов, исходя из значения переменной RSpeed
EndSub

Sub RMove ' движение робота
    If RLength < 0 Then ' определяем скорость и направление движения
        MotoSpeed = RSpeed * 10 * -1 ' если нужно ехать назад
        dir = -1 ' чтобы значение угла было положительным
    Else
        MotoSpeed = RSpeed * 10 ' если нужно ехать вперед
        dir = 1
    EndIf
    MotoAngle = RLength * dir * 36.63 ' определяем угол вращения исходя из расстояния: RLength - задается в сантиметрах
    Motor.MoveSync("AB", MotoSpeed, MotoSpeed, MotoAngle, "True")
EndSub
'_________________________________________________________

' Проверим теперь, как это работает: 
RInit()
RLength = 100 ' проехать 100 сантиметров вперед
RMove()
RLength = -100 ' проехать 100 сантиметров назад
RMove()
' Всё работает отлично! При проверке мы получили ошибку в перемещении в 1-3 мм
10После установки и подключения гироскопического датчика мы столкнулись с небольшой проблемой: гироскопы, используемые в наборах EV3 могут иметь так называемый «дрейф» — плавное, равномерное и постоянное смещение показаний. То есть, в момент когда наш робот абсолютно неподвижен гироскопический датчик будет показывать постоянно изменяющиеся значения — как если бы наш робот равномерно вращался с постоянной скоростью.

Мы написали небольшую тестовую программу для экспериментального определения наличия дрейфа гироскопа у нашего робота.

Программный код
' Постоянный вывод данных гироскопа на экран блока
Sensor.SetMode(1, 0) ' установка режима определения угловой скорости гироскопического датчика
While "True" ' бесконечный цикл опроса и вывода данных
    text = Sensor.ReadRawValue(1, 0) ' считываем угол поворота
    LCD.Clear() ' очищаем экран
    LCD.Text(1, 40, 50, 2, text) ' выводим угол поворота большим шрифтом примерно в середине экрана
    Program.Delay(100) ' пауза для вывода данных
EndWhile
11Для того чтобы вернуть робот в исходное положение после того как мы его повернули (например, руками) достаточно привязать вращение робота к показаниям гироскопа:
Программный код
MotoSpeed = 50 ' задаем среднюю скорость вращения моторов
While "True" ' в бесконечном цикле
    MotoAngle = Sensor.ReadRawValue(1, 0) ' опрашиваем гироскоп и задаем этот же угол для поворота моторов (угол, на который повернется при этом робот будет меньше угла отклонения, зафиксированного гироскопом)
    LCD.Clear() ' очищаем экран
    LCD.Text(1, 40, 50, 2, text) ' выводим угол поворота
    Program.Delay(100) ' пауза для чтения данных
    If MotoAngle > 0 Then ' в зависимости от знака угла меняем направления вращения моторов
      Motor.MoveSync("AB", MotoSpeed * -1, MotoSpeed, MotoAngle, "False") ' для вращения робота включим моторы в противоположных направлениях с одинаковыми скоростями
      ' в зависисмости от подключения моторов возможно изменение направления вращения моторов на противоположное.
    Else
      Motor.MoveSync("AB", MotoSpeed, MotoSpeed * -1, MotoAngle, "False")
    EndIf  
EndWhile

В ходе наших экспериментов мы выяснили, что в ряде случаев дрейф отсутствует вообще, либо если гироскоп опрашивать через небольшие промежутки времени, сравнивая два значения, то дрейфом можно пренебречь. У вас может получиться по-другому!

12Теперь нам нужно хотя бы примерно определить на какой угол должны поворачиваться моторы, чтобы робот поворачивался на 1О. Для этого опять понадобится провести соответствующий эксперимент. Но теперь угол поворота мы сможем определить по показаниям гироскопа, хотя и на бумажке посмотреть, что получается — не вредно, поэтому мы будем проводить измерения одновременно двумя способами и сравнивать результаты.
Программный код
Sensor.SetMode(1, 0) ' инициализировать гироскоп
text = Sensor.ReadRawValue(1, 0) ' получить показания гироскопа
LCD.Clear() ' очищаем экран
LCD.Text(1, 40, 50, 2, text) ' вывести показания на экран
Program.Delay(1000)
MotoSpeed = -50
MotoAngle = 100
Motor.MoveSync("AB", MotoSpeed * -1, MotoSpeed, MotoAngle, "True") ' повернуть на робота на некий угол
text = Sensor.ReadRawValue(1, 0) ' получить показания гироскопа
LCD.Clear() ' очищаем экран
LCD.Text(1, 40, 50, 2, text) ' вывести показания на экран

В результате относительно точных измерений (проблема в том, что гусеничные роботы в принципе не умеют идеально выполнять развороты на месте) получилось, что при угле поворота моторов в 100О робот поворачивается на 20О с возможной ошибкой до 5О в процессе самого поворота и с ошибкой до 2О в результате измерения гироскопическим датчиком.

13Следовательно, для поворота робота на 1О нужно повернуть моторы на 5О, но с учетом ошибки поворота мы будем сначала выполнять поворот моторов на 4О, затем проверять по гироскопическому датчику, что получилось, и по результатам этой проверки корректировать угол поворота, и так —  до достижения точного значения заданного угла.
Программный код
RAngle = 180
MotoSpeed = 30

Sensor.SetMode(1, 0) ' переключаем режим датчика - сборс гироскопа
Angle = RAngle ' угол доворота = RAngle
While Angle <> 0 ' пока угол доворота не 0 - нужно доворачивать
    MotoAngle = Angle * 4 ' задаем угол поворота для моторов
    If Angle > 0 Then ' если угол положительный
        Motor.MoveSync("AB", MotoSpeed, MotoSpeed * -1, MotoAngle, "False") ' повернуть на робота на угол, близкий к заданному, но чуть меньший
    Else  ' если угол отрицательный
        Motor.MoveSync("AB", MotoSpeed * -1, MotoSpeed, MotoAngle, "False") ' повернуть на робота на угол, близкий к заданному, но чуть меньший
    EndIf
    Angle = RAngle - Sensor.ReadRawValue(1, 0) ' считали показания гироскопа и определили угол доворота
EndWhile

В процессе экспериментов выяснилось, что 30 — оптимальная скорость моторов для точного поворота робота.

14Теперь библиотека подпрограмм для управления нашей LEGO-черепашкой приняла следующий вид:
Программный код
' Библиотека управления LEGO-роботом
Sub RInit ' инициализация робота
    RLength = 0 ' необходимая начальная инициализация переменных
    RAngle = 0
    RSpeed = 5 ' задаем скорость движения (если скорость не задана напрямую, по умолчанию она будет 50% от максимальной)
    dir = 1 ' переменная, "исправляющая" значение угла поворота
    MotoSpeed = RSpeed * 10 ' задаем скорость вращения моторов, исходя из значения переменной RSpeed
    Sensor.SetMode(1, 0) ' сборс и инициализация гироскопа
EndSub

Sub RMove ' движение робота
    If RLength < 0 Then ' определяем скорость и направление движения
        MotoSpeed = RSpeed * 10 * -1 ' если нужно ехать назад
        dir = -1 ' чтобы значение угла было положительным
    Else
        MotoSpeed = RSpeed * 10 ' если нужно ехать вперед
        dir = 1
    EndIf
    MotoAngle = RLength * dir * 36.63 ' определяем угол вращения исходя из расстояния: RLength - задается в сантиметрах
    Motor.MoveSync("AB", MotoSpeed, MotoSpeed, MotoAngle, "True")
EndSub

Sub RTurn ' поворот робота
    'Sensor.SetMode(1, 1) ' переключаем режим датчика - сборс гироскопа в "0" для того, чтобы нам не мешели результаты предыдущего поворота
    'Sensor.SetMode(1, 0) 
    GiroAngle = Sensor.ReadRawValue(1, 0) ' считали начальные показания гироскопа
    Angle = RAngle ' угол доворота = RAngle
    While Angle <> 0 ' пока угол доворота не 0 - нужно доворачивать
        MotoAngle = Angle * 4 ' задаем угол поворота для моторов
        If Angle > 0 Then ' если угол положительный
            Motor.MoveSync("AB", 30, -30, MotoAngle, "False") ' повернуть на робота на угол, близкий к заданному, но чуть меньший
        Else  ' если угол отрицательный
            Motor.MoveSync("AB", -30, 30, MotoAngle, "False") ' повернуть на робота на угол, близкий к заданному, но чуть меньший
        EndIf
        Angle = RAngle - (Sensor.ReadRawValue(1, 0) - GiroAngle) ' считали показания гироскопа и определили угол доворота, учитывая начальные показания гироскопа
    EndWhile
EndSub
'_________________________________________________________

' проверяем в помещении
RInit()
RLength = 133
Rmove()
RAngle = 90
RTurn()
RLength = 220
RMove()
RTurn()
RLength = 70
RMove()
RTurn()
RLength = 220
RMove
' Всё работает!
15Теперь попробуем заставить наш робот проехать по той траектории, которую проходила LOGO-черепашка на экране в тот момент, когда мы с ней временно «расстались» (пункт 4):
Программный код
' Библиотека управления LEGO-роботом
Sub RInit ' инициализация робота
    RLength = 0 ' необходимая начальная инициализация переменных
    RAngle = 0
    RSpeed = 5 ' задаем скорость движения (если скорость не задана напрямую, по умолчанию она будет 50% от максимальной)
    dir = 1 ' переменная, "исправляющая" значение угла поворота
    MotoSpeed = RSpeed * 10 ' задаем скорость вращения моторов, исходя из значения переменной RSpeed
    Sensor.SetMode(1, 0) ' сборс и инициализация гироскопа
EndSub

Sub RMove ' движение робота
    If RLength < 0 Then ' определяем скорость и направление движения
        MotoSpeed = RSpeed * 10 * -1 ' если нужно ехать назад
        dir = -1 ' чтобы значение угла было положительным
    Else
        MotoSpeed = RSpeed * 10 ' если нужно ехать вперед
        dir = 1
    EndIf
    MotoAngle = RLength * dir * 36.63 ' определяем угол вращения исходя из расстояния: RLength - задается в сантиметрах
    Motor.MoveSync("AB", MotoSpeed, MotoSpeed, MotoAngle, "True")
EndSub

Sub RTurn ' поворот робота
    'Sensor.SetMode(1, 1) ' переключаем режим датчика - сборс гироскопа в "0" для того, чтобы нам не мешели результаты предыдущего поворота
    'Sensor.SetMode(1, 0) 
    GiroAngle = Sensor.ReadRawValue(1, 0) ' считали начальные показания гироскопа
    Angle = RAngle ' угол доворота = RAngle
    While Angle <> 0 ' пока угол доворота не 0 - нужно доворачивать
        MotoAngle = Angle * 4 ' задаем угол поворота для моторов
        If Angle > 0 Then ' если угол положительный
            Motor.MoveSync("AB", 30, -30, MotoAngle, "False") ' повернуть на робота на угол, близкий к заданному, но чуть меньший
        Else  ' если угол отрицательный
            Motor.MoveSync("AB", -30, 30, MotoAngle, "False") ' повернуть на робота на угол, близкий к заданному, но чуть меньший
        EndIf
        Angle = RAngle - (Sensor.ReadRawValue(1, 0) - GiroAngle) ' считали показания гироскопа и определили угол доворота, учитывая начальные показания гироскопа
    EndWhile
EndSub
'_________________________________________________________

' Сама программа управления роботом
RInit() ' инициализируем робота
RSpeed = 6
RLength = 20 ' задаем расстояние движения
RAngle = 15 ' угол поворота - 15 градусов
For i = 1 To 24 ' рисуем многоугольник
    RMove() ' робот движется вперед
    RTurn() ' поворот на 15 градусов
EndFor
RAngle = 90 ' угол поворота - 90 градусов
RTurn() ' поворот на 90 градусов направо
RLength = 200
RMove()' отползаем направо
For i = 1 To 4 ' рисуем квадрат
    RMove() ' робот движется вперед
    RTurn() ' поворот на 90 градусов направо
EndFor
16Но и это — еще не всё. Исходя из того, что наша черепашка использует вполне ограниченный набор команд мы можем записать наш предыдущий маршрут более четко и кратко, например (M — «Move», T — «Turn»):

M — 133
T — 90
M — 220
T — 90
M — 70
T — 90
M — 220

или, если заметить, что у нас все время чередуются движения прямо и повороты, можно записать еще короче, имея в виду, что первая команда — всегда «движение»:

133, 90, 220, 90, 70, 90, 220

Мы получили обыкновенный числовой массив, который полностью задает движение нашей черепашки.

Нужно только учитывать, что первая команда — это всегда движение, потом — поворот, потом опять движение и так далее. Для того чтобы избавиться от проблем, связанных с безусловным чередованием поворотов и перемещений (например, если нам нужно, чтобы робот проехал вперед и назад не поворачивая) при такой записи для пропуска соответствующих команд можно использовать 0 («поворот на 0 градусов» или «перемещение на нулевое расстояние»). То есть, если мы хотим сначала выполнить поворот, пропустив команду «движение», мы просто должны первым в массиве поставить 0.

Теперь нам необходимо написать программный блок, который будет обрабатывать наш массив и превращать его в вызовы подпрограмм нашей библиотеки. А всё последующее программирование движения робота сведется к записи в массив соответствующих чисел. Даже команд писать не придется.

Программный код
' Этот программный модуль должен быть примерно таким:
RLen = Way[1]
RMove()
RAngle = Way[2]
RTurn()
' и так далее...

Этот код нетрудно сделать крайне компактным, используя оператор цикла.

Для того чтобы понять, что наш массив закончился (программа движения робота завершена) мы будем записывать в его конец два нуля:

133, 90, 220, 90, 70, 90, 220, 0, 0

В результате мы получим следующий код, работающий с нашим массивом:

Программный код
'Чтобы начать работать с массивом, сначала нужно заполнить его значениями
Way[1] = 133
Way[2] = 90
Way[3] = 220
Way[4] = 90
Way[5] = 70
Way[6] = 90
Way[7] = 220
Way[8] = 0
Way[9] = 0 'две нулевых команды подряд - конец "программы" для робота

i = 1 ' начинаем с первого элемента масива
While Way[i] <> 0 Or Way[i+1] <> 0 'до тех пор, пока не встретилось де нулевых команды подряд
    If Math.Remainder(i, 2) = 0 Then ' если строка нечетная...
        RAngle = Way[i] ' ...то поворот
        RTurn()
    Else
        RLength = Way[i] ' иначе - перемещение
        RMove()
    EndIf  
    i = i + 1 'переход к следующему элементу массива
EndWhile

Теперь подключим библиотеку LOGO-черепашки и посмотрим, что она нарисует нам на экране:

Программный код
' Библиотека управления LOGO-черепашкой
Sub RInit ' ("Robot Initialization" - инициализация робота)
    Turtle.Show() ' показываем черепашку на экране
    GraphicsWindow.PenWidth = 1 ' делаем толщину "следа" минимальной
    Turtle.Speed = 5 'задаем по умолчанию среднюю скорость движения черепашки
    RSpeed = 5 ' переменная управления скоростью для наших подпрограмм
    Turtle.PenDown() ' опускаем перо для включения режима рисования
EndSub

Sub RMove ' ("Robot Move" - робот движется)
    Turtle.Speed = RSpeed ' если значение переменной RSpeed изменилось - это надо учесть 
    Turtle.Move(RLength)
EndSub

Sub RTurn ' ("Robot Turn" - робот поворачивает)
    Turtle.Speed = RSpeed ' если значение переменной Speed изменилось - это надо учесть 
    Turtle.Turn(RAngle)
EndSub
' ________________________________________________________________

'заполнили массив командами для черепашки
Way[1] = 133
Way[2] = 90
Way[3] = 220
Way[4] = 90
Way[5] = 70
Way[6] = 90
Way[7] = 220
Way[8] = 0
Way[9] = 0 'две нулевых команды подряд - конец "программы" для робота

RInit() ' обязательно выполняем начальные настройки черепашки

'и запускаем обработчик массива с "программой" для движения черепашки
i = 1 ' начинаем с первого элемента масива
While Way[i] <> 0 Or Way[i+1] <> 0 'до тех пор, пока не встретилось две нулевых команды подряд
    If Math.Remainder(i, 2) = 0 Then ' если строка нечетная...
        RAngle = Way[i] ' ...то поворот
        RTurn()
    Else
        RLength = Way[i] ' иначе - перемещение
        RMove()
    EndIf  
    i = i + 1 'переход к следующему элементу массива
EndWhile

Если мы заменим библиотеку LOGO-черепашки на LEGO-робота, то получим программу, по которой робот поедет по той же траектории, используя команды из массива. Именно этого мы и добивались, создавая две наши библиотеки управления.

17А если записывать все эти числа (фактически — команды для робота) в отдельный текстовый файл, то нам больше вообще не придется переписывать саму программу для того, чтобы изменять маршрут движения робота. В результате мы получим уже почти профессиональную (пользовательскую) программу, которой нужно только менять данные, хранящиеся в отдельном текстовом файле. При этом мы сможем сначала проверить наш файл данных на LOGO-черепашке на компьютере, а потом скопировать его в папку LEGO-робота в качестве маршрута его движения. Таким образом, используя две идентичных программы (одну — на компьютере, другую — в блоке EV3), у которых различаются только библиотеки управления мы получаем законченное решение для управления роботом с предварительной проверкой программы движения на компьютере.

Файл маршрута можно сразу весь считать в массив и дальше работать с массивом, а можно просто читать построчно, вообще не используя массив. Первый вариант — более профессиональный, он позволяет за один раз считать файл и больше к нему не обращаться. Второй вариант представляется более простым с точки зрения программирования:

Программный код
str = 1 ' начинаем с первой строки файла
Way = 999 ' чтобы не сработала проверка в цикле, если первая команда - "0"
While Way <> 0 Or File.ReadLine("way.txt", str) <> 0 ' пока не встретилось две нулевых команды подряд
    Way = File.ReadLine("way.txt", str) ' читаем строку в файл
    If Math.Remainder(str, 2) = 0 Then ' если строка нечетная
        RAngle = Way ' то поворот
        RTurn()
    Else
        RLength = Way ' иначе - перемещение
        RMove()
    EndIf  
    str = str + 1 'переход к следующей строке
EndWhile

Однако, используя этот вариант, мы сразу сталкиваемся с серьёзной проблемой «совместимости»: файл на компьютере читается одной командой File.ReadLine(), а на блоке EV3 для этого существует другая команда, а самое главное — немного другой (более «серьезный») механизм: сначала файл нужно открыть для чтения командой EV3File.OpenRead("way.txt"), только после этого можно читать из него строки командой EV3File.ReadLine(file_id), которая читает строки из файла последовательно, начиная с первой. А затем надо еще обязательно надо закрывать открытый файл командой EV3File.Close(file_id) после его использования. Для этого при прямой работе с файлом (без массива) нам придется внести множественные изменения в нашу библиотеку и, вероятно, придумать какую-то дополнительную подпрограмму, используемую при завершении работы — для закрытия файла. Всё это не слишком удобно.

18Но мы можем сделать проще: в подпрограмму инициализации робота добавим функцию чтения файла в массив, соответствующую системе (отдельно для компьютера и для блока EV3). А дальше будем работать только с полученным массивом, «забыв» про файл — уже независимо от того, на компьютере мы работаем или на блоке EV3.

Чтение файла в массив построчно на компьютере выполняется несложно:

Программный код
i = 1 ' начинаем с первой строки файла (на компьютере) и первого индекса массива
File.ReadLine("way.txt", i) 'проверяем наличие и читаемость файла
If File.LastError <> "" ' если код ошибки не пустой
    TextWindow.WriteLine("Ошибка чтения файла: " + File.LastError)
    Program.End()
EndIf
While File.ReadLine("way.txt", i) <> 0 Or File.ReadLine("way.txt", i+1) <> 0 ' пока не встретилось две нулевых команды подряд
    Way[i] = File.ReadLine("way.txt", i) ' читаем строку из файла в элемент массива
    i = i + 1 'переход к следующей строке
EndWhile
Way[i]=0 ' может и одного хватит?
Way[i+1]=0
19Чтение файла на блоке EV3 выполняется чуть сложнее:
Программный код
i = 1 ' начинаем с первой строки файла (на блоке EV3) и первого индекса массива
file_ID = EV3File.OpenRead("way.txt") ' открываем файл
If file_ID = 0 Then ' файл не открывается
    EV3.SetLEDColor("RED", "PULSE") ' Хьюстон, у нас проблема!
    Speaker.Note(100, "G5", 200) ' нам нужно привлечь внимание пользователя
    Speaker.Note(100, "G5", 200)
    Speaker.Note(100, "G5", 200)
    Speaker.Note(100, "D#5", 600)
    Program.Delay(1000)
    Program.End()
EndIf
str = EV3File.ReadLine(file_id) ' читаем первую строку из файла
Way[1] = EV3File.ConvertToNumber(str) ' превращаем строку из файла в число и пишем его в элемент массива
str = EV3File.ReadLine(file_id) ' читаем вторую строку из файла
Way[2] = EV3File.ReadLine(file_id) EV3File.ConvertToNumber(str) ' превращаем строку из файла в число и пишем его в элемент массива
i = 3 ' продолжаем с третьей строки файла (на блоке EV3) и третьего элемента массива
While Way[i-2] <> 0 Or Way[i-1] <> 0 ' пока не встретилось две нулевых команды подряд
    str = EV3File.ReadLine(file_id) ' читаем строку из файла
    Way[i] = EV3File.ConvertToNumber(str) ' превращаем строку из файла в число и пишем его в элемент массива
    i = i + 1 'переход к следующему элементу массива
EndWhile
Way[i]=0 ' а может и вообще не нужно?
EV3File.Close(file_id) ' закрываем файл после завершения работы с ним
20Теперь надо все эти алгоритмы соединить и проверить в работе — в наших программах.

Программа для LOGO-черепашки:

Программный код
' Библиотека управления LOGO-черепашкой
Sub RInit ' инициализация робота
    ' заполняем массив команд из файла
    i = 1 ' начинаем с первой строки файла (на компьютере) и первого индекса массива
    File.ReadLine("way.txt", i) 'проверяем наличие и читаемость файла
    If File.LastError <> "" Then ' если код ошибки не пустой
        TextWindow.WriteLine("Ошибка чтения файла: " + File.LastError)
        Program.End()
    EndIf
    While File.ReadLine("way.txt", i) <> 0 Or File.ReadLine("way.txt", i+1) <> 0 ' пока не встретилось две нулевых команды подряд
        Way[i] = File.ReadLine("way.txt", i) ' читаем строку из файла в элемент массива
        i = i + 1 'переход к следующей строке
    EndWhile
    Way[i]=0
    Way[i+1]=0 ' может и одного хватит?

    Turtle.Show() ' показываем черепашку на экране
    GraphicsWindow.PenWidth = 1 ' делаем толщину "следа" минимальной
    Turtle.Speed = 5 'задаем по умолчанию среднюю скорость движения черепашки
    RSpeed = 5 ' переменная управления скоростью для наших подпрограмм
    Turtle.PenDown() ' опускаем перо для включения режима рисования
EndSub

Sub RMove ' робот движется
    Turtle.Speed = RSpeed ' если значение переменной RSpeed изменилось - это надо учесть 
    Turtle.Move(RLength)
EndSub

Sub RTurn ' робот поворачивает
    Turtle.Speed = RSpeed ' если значение переменной Speed изменилось - это надо учесть 
    Turtle.Turn(RAngle)
EndSub
' ________________________________________________________________


RInit() ' инициализация
' запускаем обработчик массива с "программой" для движения черепашки:
i = 1 ' начинаем с первого элемента масива
While Way[i] <> 0 Or Way[i+1] <> 0 'до тех пор, пока не встретилось две нулевых команды подряд
    If Math.Remainder(i, 2) = 0 Then ' если строка нечетная...
        RAngle = Way[i] ' ...то поворот, ...
        RTurn()
    Else
        RLength = Way[i] ' ...иначе - перемещение
        RMove()
    EndIf  
    i = i + 1 'переход к следующему элементу массива
EndWhile

Программа для LEGO-робота:

Программный код
' Библиотека управления LEGO-роботом
Sub RInit ' инициализация робота
    RLength = 0 ' необходимая начальная инициализация переменных
    RAngle = 0
    RSpeed = 5 ' задаем скорость движения (если скорость не задана напрямую, по умолчанию она будет 50% от максимальной)
    dir = 1 ' переменная, "исправляющая" значение угла поворота
    ' заполняем массив команд из файла
    i = 1 ' начинаем с первой строки файла (на блоке EV3) и первого индекса массива
    file_ID = EV3File.OpenRead("way.txt") ' открываем файл
    If file_ID = 0 Then ' если файл не открывается
        EV3.SetLEDColor("RED", "PULSE") ' "Хьюстон, у нас проблема!"
        Speaker.Note(100, "G5", 200) ' нам нужно привлечь внимание пользователя
        Speaker.Note(100, "G5", 200)
        Speaker.Note(100, "G5", 200)
        Speaker.Note(100, "D#5", 600)
        Program.Delay(1000)
        Program.End()
    EndIf
    str = EV3File.ReadLine(file_id) ' читаем первую строку из файла
    Way[1] = EV3File.ConvertToNumber(str) ' превращаем строку из файла в число и пишем его в элемент массива
    str = EV3File.ReadLine(file_id) ' читаем вторую строку из файла
    Way[2] = EV3File.ReadLine(file_id) ' превращаем строку из файла в число и пишем его в элемент массива
    i = 3 ' продолжаем с третьей строки файла (на блоке EV3) и третьего элемента массива
    While Way[i-2] <> 0 Or Way[i-1] <> 0 ' пока не встретилось две нулевых команды подряд
        str = EV3File.ReadLine(file_id) ' читаем строку из файла
        Way[i] = EV3File.ConvertToNumber(str) ' превращаем строку из файла в число и пишем его в элемент массива
        i = i + 1 'переход к следующему элементу массива
    EndWhile
    Way[i]=0 ' а может и вообще не нужно?
    EV3File.Close(file_id) ' закрываем файл после завершения работы с ним

    MotoSpeed = RSpeed * 10 ' задаем скорость вращения моторов, исходя из значения переменной RSpeed
    Sensor.SetMode(1, 0) ' сборс и инициализация гироскопа
EndSub

Sub RMove ' движение робота
    If RLength < 0 Then ' определяем скорость и направление движения
        MotoSpeed = RSpeed * 10 * -1 ' если нужно ехать назад
        dir = -1 ' чтобы значение угла было положительным
    Else
        MotoSpeed = RSpeed * 10 ' если нужно ехать вперед
        dir = 1
    EndIf
    MotoAngle = RLength * dir * 36.63 ' определяем угол вращения исходя из расстояния: RLength - задается в сантиметрах
    Motor.MoveSync("AB", MotoSpeed, MotoSpeed, MotoAngle, "True")
EndSub

Sub RTurn ' поворот робота
    'Sensor.SetMode(1, 1) ' переключаем режим датчика - сборс гироскопа в "0" для того, чтобы нам не мешели результаты предыдущего поворота
    'Sensor.SetMode(1, 0) 
    GiroAngle = Sensor.ReadRawValue(1, 0) ' считали начальные показания гироскопа
    Angle = RAngle ' угол доворота = RAngle
    While Angle <> 0 ' пока угол доворота не 0 - нужно доворачивать
        MotoAngle = Angle * 4 ' задаем угол поворота для моторов
        If Angle > 0 Then ' если угол положительный
            Motor.MoveSync("AB", 30, -30, MotoAngle, "False") ' повернуть на робота направо на угол, близкий к заданному, но чуть меньший
        Else  ' если угол отрицательный
            Motor.MoveSync("AB", -30, 30, MotoAngle, "False") ' повернуть на робота налево на угол, близкий к заданному, но чуть меньший
        EndIf
        Angle = RAngle - (Sensor.ReadRawValue(1, 0) - GiroAngle) ' считали показания гироскопа и определили угол доворота, учитывая начальные показания гироскопа
    EndWhile
EndSub
'_________________________________________________________


RInit() ' инициализация
' запускаем обработчик массива с "программой" для движения черепашки
i = 1 ' начинаем с первого элемента масива
While Way[i] <> 0 Or Way[i+1] <> 0 'до тех пор, пока не встретилось две нулевых команды подряд
    If Math.Remainder(i, 2) = 0 Then ' если строка нечетная...
        RAngle = Way[i] ' ...то поворот, ...
        RTurn()
    Else
        RLength = Way[i] ' ...иначе - перемещение
        RMove()
    EndIf  
    i = i + 1 'переход к следующему элементу массива
EndWhile

Обратите внимание, что сама программа, начинающаяся с вызова подпрограммы инициализации робота RInit(), очень небольшая и несложная. Весь функционал мы переместили в подпрограммы библиотеки.

Подведение итогов

Мы решали задачу идентичного управления двумя сходными системами:

  • LOGO-черепашкой, работающей в графическом окне MS Small Basic
  • роботом, построенным из набора LEGO EV3

Обе системы реализуют функции перемещения по плоскости, программируются на MS Small Basic и, следовательно, могут иметь идентичные управляющие команды (команды, которые приводят к выполнению одинаковых действий: движение по прямой вперед и назад на заданное расстояние и поворот на заданный угол).

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

Так как все созданные нами подпрограммы были тщательно проверены, то мы можем их использовать, что называется, «не оглядываясь» на их содержимое. Таким образом мы создали библиотеку (даже две), воспользоваться которой может любой программист. При этом его совершенно не будет интересовать, как именно написан код наших подпрограмм, главное — что они адекватно выполняют свои функции.

Обратив внимание на определенные закономерности в самих программах управления перемещением (командах), мы смогли упростить их, сведя к обычным числовым массивам. Это позволило нам хранить эти данные в отдельном текстовом файле и изменять их при необходимости, вообще не затрагивая программный код. Теперь задачу настройки маршрута для черепашки или робота может выполнить любой пользователь, не владеющий программированием.

Кроме того, обратим внимание, что у нас получилась инерциальная система управления роботом — система, которая для навигации не использует никаких внешних данных. Разумеется, в данном, простом варианте мы не можем гарантировать высокой точности перемещения робота (в отличие от LOGO-черепашки на экране), однако, это вполне приличный результат.

Кстати, потом ребята нашли проект EV3 робота с инерциальной навигацией на сайте «Карандаша и Самоделкина».

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

Но это будет уже следующий учебный проект...



Поделиться: