Управление движением робота

Лекция 9. Управление движением робота

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

Вы можете найти файлы проекта для этой лабораторной по следующему адресу:

SamplesVplHandsOnLabsLab6

Эта лабораторная научит вас:

  • Получать сигналы о стенах.
  • Получать сигналы о стенах через веб- интерфейс.
  • Движение вперед, если стены не обнаружено.
  • Опрос датчиков робота
  • Использовать редактор манифеста
  • Как использовать таймер для управления датчиком уведомления
  • Создавать активности в VPL
  • Инициализация переменных в активности
  • Создать активности для расчета нынешней позиции робота
  • Создать активности для оценки цели, если цель достигнута
  • Избегать препятствия
  • Создать активности движения на указанном направлении
  • Рассчитать угол поворота к цели
  • Создать активности GoToGoal
  • Проверка готовности
  • Отладка программы

Смотри также:

  • Датчики
  • Реактивное поведение
  • Смешивание и конкуренции
  • Проблемы смешивания и конкуренции
  • Эксперимент с датчиком Уолла

Аппаратные средства

Эта лабораторная заключается в моделировании IRobotCreate. Роботу необходимо перемещаться, поэтому компьютер должен быть расположен на IRobotCreate. Или же вы можете общаться с помощью Bluetooth.

Программное обеспечение

Эта лабораторная требует Microsoft Robotics Developer Studio, в частности, использует Microsoft Visual язык программирования (VPL).

Датчики

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

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

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

Реактивное поведение

Есть три основных подходы к поведению робота:

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

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

Смешивание и конкуренции

Для того, чтобы выполнить смешивание реактивного поведения, мы можем представлять каждое поведение как отображение вклада некоторых датчиков в мощность робота. Напомним, что мощность связана с величиной и направлением. Направление представляет собой движение робота а величина представляет собой его массу. Чтобы объединить два этих поведения для привода системы робота, можно просто выполнить сложение векторов. Это подход, при котором выигрыш поведения решает действия робота.

В этой лабораторной мы рассмотрим задачи, где робот имеет целью точку, которую он хочет достичь, но есть препятствия, которых он должен избегать. Робот имеет две реактивных формы поведения. Выход на цель просто направляет движение робота к цели, и во избежание препятствий направляет движение робота от препятствий, если они обнаружены. Поведение Выход на цель может быть представлено как сила, вектор, который указывает в направлении этой цели. Когда робот находится рядом с препятствием, во избежание препятствия главный вклад в силу дает вектор, направленный от препятствия. Величина этой силы зависит от того, насколько близко робот в настоящее время находится от объекта в соответствии с показаниями датчика. Если эти два поведения являются комбинированными, за счет конкуренции поведения с силой наибольшей величины победит такое, что робот будет переключаться между 2 линиями поведения в соответствии с показаниями его датчиков, пока не достигнет цели. Если эти два поведения будут объединены путем смешивания, векторы будут сложены, мы надеемся постепенно подойти к цели, избегая препятствий.

BlendingCompetitionPicExample – использование смешивания и конкуренции, чтобы избежать препятствий при подходе к цели.

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

Проблемы смешивания и конкуренции

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

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

Эксперимент с датчиком стены

Имеется инфракрасный датчик IR (ИК) " Wall Sensor" на передней правой стороне робота. Этот датчик испускает инфракрасный сигнал и смотрит, как он отражается обратно к датчику, чтобы обнаружить, насколько близко робот к стене. Мы будем использовать этот датчик, чтобы избегать препятствий. Учитывая положение и диапазон датчика, это не всегда будет возможно - он предназначен для обнаружения стены справа, а не для обхода препятствий. Мы начинаем с экспериментов с датчиком, чтобы получить представление об ассортименте его возможностей. Руководство открытого интерфейса IRobotCreate содержит два пути для получения информации от Sensor’а стены. Этот сенсор стены дает сигнал, в диапазоне 0-4095. На практике, однако, мы ожидаем, что вы не увидите числа, близкие к верхнему пределу.

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

Чтобы начать работу, откройте VPL - Язык программирования приложений.

Шаг 1: Добавить IRobotCreate

Найдите IRobot в Services Panel и добавьте IRobot Create / Roomba на диаграмму. Для того чтобы использовать Датчик стены IRobot нужно поместить либо IRobot Create / Roomba или IRobot Create Lite на диаграмме.

Шаг 2: Получение сигнала стены

Добавить блок Calculate из Basic Activities Panel и подключите круглый выход из IRobotCreateRoomba на входящие подключения блока Calculate. Напомним, что в VPL круглый контакт используется для уведомления. От круглого выхода iRobotCreateRoomba вы можете получить уведомление от службы. Когда вы установите связи, откроется диалоговое окно. Проверьте диалог, чтобы увидеть параметры, которые доступны. Мы хотим получить обновление для сигнала стены. Одним из вариантов является UpdateBumpsCliffsAndWalls. Выберите эту опцию. Теперь поместите курсор в поле Calculate. Выпадающее меню доступных значений должно появиться. Как вы можете видеть, обновление сигнала доступно. Это показано на следующем рисунке.

UpdateBumpsCliffsAndWalls - информацию доступную из UpdateBumpsCliffsAndWalls

Если навести указатель мыши на переменную Wall в раскрывающемся меню, вы увидите, что эта переменная имеет логический тип. Это булев сигнал датчика стены робота, а не целое значение сигнала стены, который мы ищем. Мы можем получить Сигнал стены от UpdateCliffDetails.

Щелкните правой кнопкой мыши на связи IRobotCreateRoomba с блоком Calculate. Выберите из появляющегося меню Connections. Теперь вы можете выбрать UpdateCliffDetails вместо UpdateBumpsCliffsAndWalls. Теперь, когда вы поместите курсор в поле WallSignal вы увидите Int в выпадающем меню. Выберите эту переменную, или введите текст WallSignal. Ваша схема должна выглядеть следующим образом:

StepWallSignal - диаграмма с обновлением WallSignal

Шаг 3: Установите манифест для создания и подключения робота

Дважды щелкните на IRobotCreateRoomba. В Set Configuration Drop Down выберите Use a manifest. Дополнительное окно появится, щелкните на Import Manifest, а затем выберите IRobot.Manifest.xml. Если вы еще не сделали этого, сейчас самое время подключить робот к компьютеру. Прежние лабораторные содержат подробную информацию о настройке робота.

Шаг 4: Проверьте сигнал стены через Web-интерфейс

Начните с запуска диаграммы. Появится Web - интерфейс для подключения робота. Убедитесь, что порты и способ связи и т.п., являются правильными, а затем нажмите кнопку Connect. Теперь вы можете экспериментировать с WallSignal. Начните работу с роботом на достаточном расстоянии от стены. Нажмите кнопку "Обновить" в браузере. Теперь прокрутите вниз и смотрите на все показания датчика. Обратите внимание на значение сигнала датчика стены - он должен быть равен нулю. Теперь поместите ваш робот у стены, чтобы его датчик был в одном дюйме от нее. Нажмите обновить и еще раз взгляните на сигнал датчика стены. Значение должно быть выше. Эксперимент этот - чтобы получить представление о диапазоне значений.

Некоторые вещи, которые вы можете проверить:

  • Насколько близко датчик должен быть от стены, чтобы получить ненулевое значение?
  • Если робот прямо лицом к стене (фронт), что является значением сигнала стены?
  • Если датчик вплотную к стене, что показывает датчик?
  • Влияет ли цвет стены на показания датчика?

Step 5: Логика движения вперед, пока стена не обнаружена

Из панели Activities поместите на диаграмму блок If Statement. Соедините выход блока Calculate со входом блока If . Введите условие блока If - value > 5 или другое целое значение по вашему выбору. Это придаст значение WallSignal true если сигнал больше 5.

Из панели Services добавьте Generic Differential Drive. Соедините выход If с Drive и выберите SetDrivePower . В Data Connections, выберите checkbox Edit values directly внизу слева (см. Ниже), и введите значение мощности для левого и правого колес -0.1 . Это заставит робот двигаться назад.

Теперь наша диаграмма заставит робот двигаться назад, если сигнал WallSignal больше 5. Теперь мы хотим заставить робот двигаться вперед, если это не так. Мы хотим использовать тот же differential drive (не новый независимый сервис).Один из способов достичь этого – выбрать GenericDifferentialDrive на нашей диаграмме и нажать Ctrl-c, затем Ctrl-v чтобы вставить копию. Если мы выберем копию, откроем свойства и поле имени. Там значится GenericDifferentialDrive. Если вы измените имя на другое, например MyDrive, имена обоих блоков GenericDifferentialDrive изменятся.

Соедините второй блок GenericDifferentialDrive с выходом else и выберите SetDrivePower. В Data Connections установите Edit values directly, и введите значение мощности для обоих колес 0.1. Это заставит робот двигаться вперед. Диаграмма примет вид:

Можно упростить диаграмму удалив блок Calculate. Чтобы сделать это, нажмите на связи между Calculate и If t, затем удалите ее. Отсоединим блок Calculate от IRobotCreateRoomba и переместим конец связи к блоку If. Чтобы сделать это, выберите соединение между двумя блоками. Затем перетащите конец, присоединенный к блоку Calculate к блоку If. Это позволит соединить IRobotCreateRoomba с If без необходимости обновлять уведомление UpdateCliffDetail. Теперь измените value в If на WallSignal и все сделано.

Нужно выбрать манифест для GenericDifferentialDrive. Дважды нажмите на одном из блоков (неважно на каком – они являются копиями). Выберите Use a Manifest, затем импортируйте IRobot.Drive.Manifest.xml .

Шаг 6: Запуск программы с опросом сенсоров

Запустим программу! Когда загружается web-interface, заметьте, что интервал опроса автоматически установлен равным 201 миллисекунд. Пока можно оставить эту величину. Этот интервал определяет частоту, с которой робот опрашивает сенсоры и посылает уведомления. Иногда эта частота может быть достаточной, иногда мы хотим получить обновленное значение независимо от того, с какой частотой робот опрашивает сенсоры.

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

Шаг 7: Использование Manifest Editor для получения уведомлений сенсора

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

В VPL, прежде чем запустить программу, вы выбирали манифест для GenericDifferentialDrive. Этот манифест - XML файл, обеспечивающий информацию о сервисах, необходимых для запуска драйвера. Manifest editor можно использовать для конфигурирования этих сервисов, установления отношений между ними, добавления новых сервисов и т.д. Здесь мы увидим, как можно конфигурировать сервис.

Из Start Menu откройте Microsoft Dss Manifest Editor. Из меню File выберите Open и перейдите в директорию Samples\Config, выберите iRobot.Drive.manifest. Нажмите на сервис irobot, и его свойства появятся в правой панели.

ManifestEditorIRobot - This is what the Manifest Editor should currently look like.

Мы хотим конфигурировать сервис IRobot. Начнем с изменения имени на IRobotWallNotify. Затем изменим имя робота, назовем его MyRobot, убедитесь, что порт SerialPort установлен надлежащим образом, что значение BaudRate равно 57600 и что IRobotModel установлено Create. Убедитесь, что WaitForConnect не выбрано. Если это свойство выбрано, то в момент старта программы должен загрузиться web-interface и ждать, пока пользователь нажмет кнопку Connect. Когда опция не выбрана, робот автоматически свяжется, если все установки правильны, и web-interface загрузится, только если возникнут проблемы.

Чтобы отключить опрос сенсоров, установите PollingInterval в -1 . Мы должны определить, обновление какого сенсора мы хотим получить (или не хотим). Нажмите на знаке + возле CreateNotifications. Появится выпадающее меню. Поскольку нас интересует сигнал от стены, WallSignal, выберем из меню AllCliffDetail. Мы закончили с конфигурацией сервиса.

Нужно сохранить настройки нового манифеста, но мы не хотим переписывать старый. В меню File выберем Save As и сохраним манифест как iRobot.Drive.WallNotify.manifest.xml в новой директории в samples\config.Возникнет новая директория со всеми нужными файлами. Перейдите в эту директорию. Вы увидите несколько файлов. Файл irobot.config.xml содержит информацию об установленной конфигурации IRobot. Файл iRobot.Drive.WallNotify.manifest.xml – новый файл манифеста, который вы только что создали. Теперь можно выбрать этот манифест в VPL. Перейдем в директорию samples\config. В ней находится файл irobot.config.xml. Если открыть его, увидите исходную конфигурацию, с PollingInterval равным 201 и т.д..

Шаг 8: Попытайтесь запустить диаграмму с уведомлениями сенсоров

Сначала установим GenericDifferentialDrive чтобы использовать манифест iRobot.Drive.WallNotify.manifest который мы только что создали.

Когда мы редактировали манифест, мы конфигурировали сервис iRobot с помощью манифеста драйвера. Этот манифест запускает сервис iRobot. Теперь у нас есть iRobotCreateRoomba запускающий отдельный сервис iRobot. Чтобы зафиксировать это, нажмите iRobotCreateRoomba, и в панели Properties выберите IRobotWallNotify in iRobot.Drive.WallNotify.manifest из выпадающего меню Manifest. Теперь iRobotCreateRoomba установлен для использования сервиса iRobot запускаемого драйвером.

Запустите программу, пусть она поработает некоторое время. Вы заметите, что робот запускается без нажатия на connect! Теперь посмотрим, чем отличается поведение робота, когда происходит опрос.

Могут возникнуть некоторые проблемы в поведении робота. Робот может быстро колебаться между двумя вариантами поведения. Это случается потому, что мы получаем обновление уведомлений сенсоров гораздо чаще чем через 201 миллисекунду (интервал опроса по умолчанию). Возможно, сенсоры посылают уведомления быстрее, чем программа может их обработать. Если робот врезается в стену, возможно уведомления сенсора подпирают друг друга. Каждый раз, когда мы получаем уведомления сенсора, мы посылаем команду драйверу робота. Если этот процесс медленнее, чем поступление нового уведомления, робот действует на основе старой информации. Возможно столкновение со стеной.

В следующем разделе мы увидим, как игнорировать некоторые уведомления сенсора, чтобы не возникло отставания.

Шаг 9: Использование таймера для игнорирования некоторых уведомлений сенсоров

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

Сначала мы должны создать переменную для счетчика и задать ей нулевое исходное значение.

Затем нужно инициализировать таймер. Из панели Services перетащите Timer. Мы хотим чтобы таймер работал с интервалом 50mms (можно попробовать другие интервалы). Чтобы выполнить это, добавьте блок Data с целым значением 50. Присоедините его, используя SetTimer, выберите value в Data Connections .

Когда таймер установлен, нужно изменять значение переменной Counter. Для этого скопируйте и вставьте таймер и соедините его с блоком Calculate, выбрав TimerComplete. В блоке Calculate установите переменную Counter и добавьте к ней 1, написав state.Counter + 1 . Результат установите в значение переменной Counter как на рисунке.

Когда таймер завершит работу, нужно иметь возможность запустить его снова. Используйте блок Data и передайте его значение Timer (таймер можно скопировать) используя SetTimer . Соедините блок Data с TimerComplete как это делалось раньше.

Теперь нужно использовать переменную Counter чтобы решить, следует ли игнорировать обновление данных сенсора или оценивать сигнал WallSignal и посылать соответствующую команду драйверу GenericDifferentialDrive . Добавьте блок If на диаграмму. Условие, которое мы хотим установить: state.Counter >= 4 (можно попробовать другие). Мы хотим проверить это условие по получении уведомления от UpdateCliffDetail . Мы используем условие >= вместо > чтобы оно проверялось несколько раньше, чем робот получит первое уведомление сенсора. Если условие даст true, нужно перезапустить счетчик. Добавьте логический элемент на диаграмму для этого. Одна ветвь управления завершена.

Если условие даст true, нужно также воздействовать на уведомление сенсора. Нужно чтобы поток управления перешел с этого условия на блок If сигнала WallSignal. Если мы соединим блок If с WallSignal к выходу If для Counter блок CounterIf передаст UpdateCliffDetail второму If. Так мы будем в состоянии принять сигнал WallSignal как требуется. Диаграмма после этих изменений будет такой:

Шаг 10: Запуск диаграммы

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

Реализация конкуренции

Мы начинаем преобразовывать диаграмму так, чтобы управлять движением робота к цели, обходя препятствия. Чтобы реализовать это поведение, нужно начать его движение из определенной точки и в заданном направлении. Будем считать, что ориентация 0 градусов соответствует движению робота вдоль Х оси. Расстояния измеряем в метрах.

Поскольку ожидается, что диаграмма конкуренции окажется большой, разумно вначале спланировать полный поток управления. Каждый раз, когда срабатывает таймер, мы будем проверять, достигли робот цели. Если да, мы остановим движение. Если нет, мы сравним величину вектора движения к цели (go-to-goal) с величиной вектора, описывающего обход препятствия (avoid-obstacle). Результат будет определять движение робота - либо по направлению к цели, либо от обнаруженного препятствия. В ходе этого процесса нужно сохранять направление из текущего положения робота. Возможно получить новое положение робота из его старого положения и данных о повороте, используя правила тригонометрии.

Шаг 1: Запуск новой VPL диаграммы и создание активности

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

Откройте новую VPL диаграмму, получим чистый исходный лист. Чтобы создать новую активность, перетащите блок Activity на пустую главную Diagram из панели Basic Activities. В панели Properties справа измените имя активности в обоих полях на CompeteHelper.

По умолчанию VPL создает Activity со страницей Start и одним Action. Любой код со стартовой страницы будет запущен в первый раз с вызовом некоторого действия из Activity. Это позволяет инициализировать необходимые для активности переменные. Когда вы соединяетесь в VPL с активностью, вы выбираете, какое действие вы хотите вызвать. Мы уже видели это раньше! Когда вы соединялись с GenericDifferentialDrive например, вы выбирали SetDrivePower, а затем подавали на вход. Когда вы создаете собственную активность, нужно определить действия и их входы и выходы.

Дважды нажмите на блоке CompeteHelper и откройте активность. По умолчанию VPL открывает активность на странице Action. Можно выбрать страницу Start из выпадающего меню сверху. Для редактирования действий и их входов и выходов нажмите на кнопку возле меню (см. скриншот).

Шаг 2: Инициализация переменных на стартовой странице

Перейдите на стартовую страницу Start активности CompeteHelper и инициализируйте переменные показанные на следующей диаграмме. Эти переменные понадобятся для определения текущего положения. Определение текущего положения используя информацию о том, насколько повернулись колеса, называется dead-reckoning (счисление пути). Счисление пути не дает стопроцентной точности, в приложениях важно знать точное расположение робота, это достигается использованием определения точки на поверхности.

Шаг 3: Обновление данных о текущем положении робота и определение достижения цели

Мы хотим создать несколько действий в CompeteHelper. Цель первого – получить данные о текущем положении и определить, достигнута ли цель.

Щелкните на иконке возле выпадающего меню и откройте диалог Actions и Notifications. Переименуйте действие с именем Action, UpdatePosEvalDone. Добавьте входную переменную типа int. Входом будет расстояние, возвращаемое сенсором Pose (он сообщает, как далеко продвинулся робот), назовем переменную int, PoseDistance. Введем выходную переменную типа bool названную Done так как мы хотим узнать, достиг ли робот цели.

Шаг 4: Создание активности для определения текущего положения робота

Мы можем инкапсулировать часть нашего действия по определению положения в другой активности. Вход этой активности будет получать сохраненные (x,y) координаты, текущее направление в градусах и перемещение в метрах.

Добавьте блок Activity к своему действию и назовите его CalcCurrentPosition. Дважды нажмите на активности, чтобы редактировать ее. Нужно добавить следующие входные переменные типа double: XPrev, YPrev, CurrentHeading и DistanceMoved, и выходные переменные типа double: XNew и YNew.

Чтобы рассчитать текущее положение робота используем тригонометрию. Следующая диаграмма показывает пример расчета во втором квадранте, т.е. когда текущее направление больше или равно 90 градусам, но меньше 180. Расстояние h соответствует входу DistanceMoved нашей активности. Нужно понять, как рассчитать XNew и YNew в каждом из случаев. Между ними есть небольшое различие.

Поможем начать создавать код для этих расчетов. Входные величины нужно будет использовать много раз, поэтому их нужно хранить в переменных. Доступ к входным величинам получаем соединением с квадратом на левом краю действия. Мы хотим присвоить каждое значение входных величин переменным. Чтобы извлечь значение одной из входных величин, соедините квадрат с блоком calculate а затем напишите имя нужного входа. Сделайте это для каждого входа и присвойте рассчитанное значение переменной , диаграмма должна выглядеть так:

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

Теперь для каждой ветви If напишем код для вычисления XNew и YNew и присвоения их переменным. Используем блок merge для комбинирования управления ото всех ветвей блока If. Затем соединим выход merge с выходным квадратом действия. Надо будет редактировать результирующие значения. Следует пропустить state.XNew и state.YNew. Чтобы получить sine и cosine используем MathFunctions. При этом Sine и Cosine на входе должны иметь значения в радианах, нужно конвертировать градусы в радианы. Попробуйте записать ваш VPL код прежде, чем посмотрите на схему.

Конечная диаграмма для CalculateCurrPosition приведена ниже. Если вы хотите увидеть, какие величины проходят по всем связям, можно открыть файл .mvpl для этой лабораторной и посмотреть код. Эта диаграмма довольно велика. Можно выделить часть каждого условия If в отдельную активность.

Шаг 5: Создание активности, определяющей достижение цели

Вернемся к действию UpdatePosEvalDone активности CompeteHelper, и создадим новую активность. Мы хотим, чтобы эта активность определяла, достиг ли робот цели (в пределах некоторой ошибки). Входом этой активности будут текущие значения x и y координаты робота и координаты x и y цели. Выходом будет булева величина, определяющая, достигнута ли цель.

Добавьте новую активность и назовите ее EvalAtGoal. Дважды нажмите на ней для редактирования. Нужно добавить входы и выход как на схеме ниже.

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

Диаграмма работает так:

  • вычисляется расстояние между текущим положением и целью по Х и Y.
  • если расстояние по обоим направлениям больше 0.1, блок merge получит значение false.
  • если расстояние по обоим направлениям меньше 0.1, сработает блок join, блок merge получит значение true.
  • входное значение merge передается блоком merge, и результат Done присваивается этой величине.

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

Шаг 6: Завершение действия CompeteHelper - UpdatePositionEvalDone

Теперь, когда созданы вспомогательные активности, мы готовы завершить действие UpdatePositionEvalDone активности CompeteHelper.

Вспомним, что входом активности было значение Distance даваемое сенсором робота Pose. Сервис iRobot предлагаемый RDS использует информацию, возвращаемую роботом, это пройденное расстояние в миллиметрах. Если почитать руководство к роботу можно подумать, что каждый раз, когда используется действие GetSensors, это значение будет изменено. Однако сервис iRobot не переписывает значения при каждом вызове, вместо этого значения могут аккумулироваться. Поэтому входное значение PoseDistance представляет расстояние, пройденное роботом со старта.

Прежде всего нужно конвертировать PoseDistance в метры, так как в метрах вычисляются координаты. Затем нужно определить, как далеко продвинулся робот с последнего момента определения его положения. Это можно сделать используя ранее введенную переменную - OldDistanceTravelled.

Конвертируем в метры просто деля на 1000. Когда поток управления достигнет join, мы имеем всю необходимую для вычисления текущего положения робота информацию.

Соедините выход блока join со входом CalcCurrentPosition и выберите Edit values directly в свойствах Data Connection. Мы хотим передать то, что было записано прежде – текущие значения X и Y, направление, которое также записано (каждый раз, когда меняется направление движения, записывается текущее направление), и перемещение, которое вычисляется. Свойства Data Connection будут выглядеть так:

Задав входные значения, пора определить выходные. Мы получим значения XNew и YNew, это можно сделать, используя блоки Calculate, и присвоив эти значения переменным состояния XCurr и YCurr. Эти потоки управления затем нужно объединить.

Затем присвоим OldDistanceTravelled переменной NewDistanceTravelled и назовем EvalAtGoal. Вход EvalAtGoal это переменные состояния state.XCurr, state.YCurr, state.XGoal, state.YGoal. Выход Done переменной EvalAtGoal должен быть подан на выход Done, действия.

Действие UpdatePositionEvalDone в CompeteHelper теперь завершено!

Шаг 7: Действие по обходу препятствий

Добавим новое действие к нашей активности CompeteHelper – обход препятствий. Это действие будет инициировано когда поведение avoid-obstacle behavior "победит" поведение go-to-goal. Цель этого действия – заставить робот двигаться от препятствия.

Чтобы добавить новое действие к активности CompeteHelper, откройте диалог Actions and Notifications и на вкладке Actions панели Actions нажмите кнопку add. Назовите действие AvoidObstacle. Это действие не имеет входов и выходов. Оно должно вычислять направление поворота робота так, чтобы оно указывало от препятствия и заставляло робот двигаться в этом направлении.

Робот должен двигаться в направлении, противоположном направлению на препятствие. Чтобы определить это направление, нужно учесть положение WallSignal на роботе и текущий угол поворота робота. На роботе измерьте угол между осью робота и датчиком стены. Должно быть около 50 градусов – но может быть иначе. Мы будем работать со значением 50 градусов.

Когда текущее направление движения робота 0 градусов, мы должны выполнить расчет направления, показанный на схеме. Если робот уже повернулся на некоторый угол, нужно добавить 130 градусов к текущему углу поворота and take the result modulo 360 degrees. Это новое значение будет присвоено переменной CurrentRotation а старое значение CurrentRotation будет записано в переменной OldRotation. Создайте VPL код для выполнения этих расчетов и задайте переменную.

Теперь, когда мы рассчитали направление, нужно начать двигаться. Для этого создадим новую активность, снова используем код, написанный для движения go-to-goal.

Шаг 8: Создание активности для движения в заданном направлении

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

Действие очень простое. Нужно передать RotateDegrees активности GenericDifferentialDrive передавая DesiredHeading-OldHeading в качестве Degrees и число 0.1 в качестве значения мощности Power. Затем просто вызвать SetDrivePower в GenericDifferentialDrive и установить обоим колесам одинаковую мощность Power. Малое значение мощности устанавливается для замедленного движения и обнаружения препятствий.

Шаг 9: Завершение действия AvoidObstacle

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

Свойства Data Connections для входа в DriveOnDirection должны определять state.CurrentRotation как DesiredHeading и state.OldRotation как OldHeading. Конечная диаграмма должна выглядеть так:

Шаг 10:Запуск действия GoToGoal

Действие GoToGoal будет вызвано для инициализации движения робота, когда победит поведение go-to-goal. Это последнее действие, необходимое в активности CompeteHelper. GoToGoal не требует входов и выходов. Добавьте действие в VPL как два предыдущих.

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

Шаг 11: Расчет угла поворота к цели

Действие этой активности имеет 4 входа - текущие X,Y робота и X,Y цели, а выход один - угол поворота. Создайте эти входы и выход как раньше.

Рассчитаем направление движения робота (поворот измеряется от Х - оси). На схеме показан вариант расчета для второго квадранта.

Запишите VPL код для расчета тэта используя арктангенс из MathFunctions а затем переведите результат в градусы. Такие же вычисления тэта можно сделать для любого квадранта, беря абсолютные значения разностей по Х и Y координатам.

Понадобится блок If для определения, в каком квадранте проводятся вычисления. Блок If можно использовать и для обработки граничных случаев. Добавьте If на диаграмму и условия для каждого случая. Соедините блок Calculate с каждым выходом для расчета правильного значения тэта . Наконец, merge выходы всех блоков расчета и используйте значение на выходной связи блока merge для определения результирующего значения переменной Rotation.

После всего диаграмма примет вид: .

Шаг 12: Завершение действия GoToGoal

Мы создали активность CalculateGoalRotation, кодировка действия GoToGoal становится очень простой. Нужно просто записать state.CurrentRotation в переменную состояния OldRotation, используя CalculateGoalRotation для расчета угла движения, сохранить его в качестве нового CurrentRotation, а затем использовать активность DriveOnDirection.

Заметьте, что нам удалось сделать диаграмму GoToGoal очень простой и понятной благодаря использованию активностей.

Мы записали три действия для активности CompeteHelper: UpdatePosEvalDone, AvoidObstacle, и GoToGoal. Мы завершили создание активности CompeteHelper. Теперь можно вернуться к главной диаграмме и все объединить!

Шаг 13: Код проверки готовности робота

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

Step 14: План управления

Нужно спланировать поток управления для главной части диаграммы. Один из возможных вариантов.

  • Когда срабатывает таймер: обновить позицию робота с помощью информации сенсора и проверить, достигнута ли цель.
  • Если достигнута – остановить робот.
    • Нет - определить, какое поведение определяет движение робота:
      • Если WallSignal >= threshold то значение вектора препятствия avoid-obstacle превосходит значение go-to-goal. Значит, управлять будет AvoidObstacle в правильном направлении, затем таймер сработает снова.
      • Если сильнее поведение go-to-goal.
        • Если робот не движется в настоящий момент к цели, GoToGoal двигает робот в правильном направлении, затем таймер сработает снова.
        • Если робот движется в настоящий момент к цели, таймер должен сработать снова.

Шаг 15: Главная диаграмма

Первым делом добавим инициализацию. В нашем плане управления мы видели, что полезно записывать текущее состояние робота. Добавим bool переменную CurrBehaviorGoal на диаграмму и установим ее значение false (робот стартует без установленного поведения).

Нужно, чтобы таймер сработал в самом начале. Мы сделаем это после того, как RobotReady установлено в true. Добавьте этот код в диаграмму.

Теперь вы готовы работать с основным потоком управления.

Когда срабатывает таймер, мы хотим запросить GetSensors робота iRobotCreateRoomba. Сделайте копию только что установленного таймера и соедините его порт уведомлений (круглый) с копией блока iRobotCreateRoomba. Первое показание сенсора, которое нам нужно – расстояние, которое прошел робот. Мы получим его из AllPose. Так что вход в GetSensors должен быть перечислимый тип CreateSensorPacket.AllPose. Это сообщает GetSensors какой сенсор мы запрашиваем. Нам нужен также WallSignal, создайте еще копию блока iRobotCreateRoomba и соедините ее с уведомительным портом таймера. Мы хотим на входе CreateSensorPacket.AllCliffDetail. Когда вы соединили, скажем, блок Calculate, с выходом iRobotCreateRoomba, вы получите нужную информацию.

Мы можем подать резльтат запроса AllPoseGetSensors на действие UpdatePositionEvalDone активности CompeteHelper. Для этого соедините iRobotCreateRoomba с блоком CompeteHelper и выберите действие. В Connections, выберите опцию Get Sensors которая дает информацию Pose, а в Data Connections передайте value.Distance. Активность CompeteHelper затем примет участие в определении текущего положения и определит, достигнута ли цель.

Теперь соедините выход блока CompeteHelper с блоком If. Если Done, передайте SetDrivePower в GenericDifferentialDrive. Наконец, из панели Services добавьте SimpleDialog и соедините его с выходом success блока GenericDifferentialDrive. В Data Connections значение изменяется напрямую, так что можно отразить string"Done!" в диалоге.

Далее, добавьте join чтобы объединить управляющие потоки от условия else блка If, и результат запроса GetSensors для WallSignal.

Выход join соедините с новым блоком If , который определяет вызов AvoidObstacle в CompeteHelper, GoToGoal в CompeteHelper, или продолжать как идет. Обновите переменную CurrBehaviorGoal и используйте слияние для объединения потоков управления от обеих ветвей.

Последний шаг – перезапустить таймер после слияния. После этого программа завершена!

Шаг 16: Выбор манифеста и тестирование

Теперь нужно выбрать манифест для GenericDifferentialDrive и для iRobotCreateRoomba. Мы не будем использовать манифест уведомлений, созданный ранее, мы используем iRobot.Drive.Manifest.xml. Когда уведомления включены, робот создает много меньше значений Pose чем следует. Помогает включение опроса, но ваше счисление пути будет гораздо более точным если выключить все потоки уведомлений. В web-interface который позволяет связаться с роботом, можно уменьшить интервал запросов до 50ms. Это минимальное значение которое может быть использовано при беспроводной связи с роботом.

После выбора манифеста можно тестировать программу. Можно установить различные пороговые значения для сенсора WallSignal и разные значения для Timer.

Сенсор стены находится только слева, поэтому робот может стакиваться с препятствиями. Поэтому можно добавить бампер.

Отладка программы

В такой большой программе будет много ошибок. Прежде всего нужно проверить следующее.

Нужно проверить значения по умолчанию и возможные неверные значения на связях. Легко забыть установить значения в Data Connections. Нажмите на линии связи особенно на линии связи с квадратными контактами активностей.

Дебаггер позволяет увидеть значения переменных, что полезно для обнаружения других ошибок. Для запуска дебаггера нажмите на стрелке рядом со стрелкой Run. Будет запущен web-browser содержащий графическое представление выполняемого кода. Дебаггер интуитвно понятен в применении..

Если программа зависнет, это может быть вызвано тем, что поток управления, который она пытается запустить, является эксклюзивным по отношению к текущему. Мы обсудим, как конкуренция и эксклюзивность обрабатываются VPL в работе 7. Your code may also get stuck if some path in an activity fails to return.

Complete Helper

CalculateGoalRotation

EvalAtGoal

DriveOnDirection

Управление движением робота