Управление движением робота с одного джойстика

Управление движением робота с одного джойстика

В данной статье будет рассмотрен алгоритм управления 4-х колесным роботом с помощью одного джойстика.

Принцип действия

Его составной частью будет алгоритм «архивации» скорости.

Для начала рассмотрим принцип движения нашего робота (немного известной вам теории). Его ходовая часть состоит из 4-х мотор-редукторов (по одному на каждое колесо). Поэтому движение происходит следующим образом:

Вперед. Все 4 двигателя вращаются вперед с равной скоростью.

Назад. Все 4 двигателя вращаются назад с равной скоростью.

Плавный поворот направо. Правые двигатели вращаются медленнее левых. Но все 4 вращаются в одну сторону.

Плавный поворот налево. Левые двигатели вращаются медленнее правых. Но все 4 вращаются в одну сторону.

Резкий поворот направо. Левые двигатели вращаются вперед, правые назад. Если все двигатели вращаются с одной скоростью, то робот развернется на месте.

Резкий поворот налево. Правые двигатели вращаются вперед, левые двигатели вращаются назад. Если все 4 вращаются с одной скоростью, то робот развернется на месте.

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

Схема соединения

Для передачи данных от пульта к роботу я использую радио модули NRF24L01. Они передают каждый раз массив значений. Чем меньше вес этого массива, тем быстрее будет передан сигнал и тем меньшую скорость передачи данных мы можем задать модулю, что уменьшит его энергопотребление. Поэтому данные с джойстика управления будем записывать в 2 ячейки массива типа byte.

Теперь рассмотрим как считываются данные с джойстика на пульте. VCC модуля соединяем с 5V Ардуино, GND с GND, потенциометр горизонтальной оси с A0, вертикальной с A1, кнопку не подключаем, так как задействовать не будем. Схема приведена ниже. Вид модуля может отличаться, но пины будут такими же (кроме случаев с отдельным подводом VCC и GND к каждому потенциометру и кнопке).

Считав данные с джойстика функциями analogRead(A0) (можно заменить А0 на horizontalSticPin ) и analogRead(A1) (можно заменить А1 на verticalSticPin ) получим следующие диапазоны значений. Вперед 0 на вертикальной оси, назад 1023 на вертикальной оси, резкий поворот вправо 0 на горизонтальной оси, резкий разворот влево 1023 на горизонтальной оси. Значения между ними будут давать градации поворотов: положение 0,0 даст плавный правый поворот вперед, а значение 1023,1023 даст плавный левый поворот назад.

Алгоритм

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

Вот его принцип:

  1. Вводим следующие константы:
    – verticalSticCenter – значения, соответствующие центральному положению джойстика в вертикальной оси (средняя скорость робота равна 0);
    – horizontalSticPin – значения, соответствующие центральному положению джойстика в горизонтальной оси (скорость разворота равна 0)
    – k – коэффициент отклонения, от центральных положений, позволяет исключить влияния «шумов» на показания в положении полной остановки робота)
  2. Считываем показания вертикальной оси потенциометра в переменную speed , хранящую значение средней скорости. Считанные значения нужно перевести в диапазон типа byte , разделив на 4 (можно использовать функцию map ), в таком случае средняя скорость будет изменяться от 255 до 0. Затем вычесть из них значение verticalSticCenter , теперь средняя скорость изменяется от 128 до -128. Но для удобства расчетов хотелось бы видеть изменения скорости в диапазоне от -128 до 128 (тогда центральное положение будет давать 0, что очень важно), для этого делим показания на единицу.
  3. Смотрим, если значения относительной скорости отличаются от 0 менее чем на k (джойстик находиться в центральном положении по вертикальной оси), то приравниваем ее к 0.
  4. Считываем значения горизонтальной оси в переменную horizontal_value так же приведя к диапазону byte , будем называть их скоростью поворота.
  5. Если значения скорости поворота отклонились более, чем на k от центра в право, то скорость левого двигателя приравниваем к сумме средней скорости и величине отклонения в угловой скорости от условного 0 (вычитаем значения скорости поворота из значений ее центра), а скорость правого к их разности.
  6. Если значения скорости поворота отклонились более, чем на k от центра влево, то скорость левого двигателя приравниваем к разности средней скорости и величине отклонения от условного центра (из скорости поворота вычитаем значения условного 0), а правого к их сумме.
  7. Не выполнение условий пунктов 5 и 6 говорит о том, что значения скорости поворота лежат в пределах центра (условного 0), в этом случае приравниваем скорости обоих двигателей к средней скорости.
  8. Теперь необходимо ограничить полученные значения скоростей, что бы по модулю они не выходили за половину максимального значения типа byte для предотвращения переполнения переменных в пункте 9. Просто сравним значения с константно-объявленной границей, и если они больше, то приравняем их к ней.
  9. Сейчас скорости двигателей лежат в диапазоне от -128 до 128, но мы хотим хранить их в массиве типа byte , который не может хранить отрицательные числа, поэтому «зашифруем их»:
    0 элемент массива data будет хранить скорость левого двигателя, а 1 скорость второго.
    – Если скорость будет отрицательной, мы положим в data ее модуль, а если положительной, то прибавим к ней половину диапазона значений типа byte . Таким образом значения от 0 до 128 хранят скорость вращения назад, а значения от 128 до 255 вперед.

Коды проекта

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

Теперь переходим непосредственно к коду проекта.

Напишем этот алгоритм в функции getValue . Чтобы не перегружать код в комментариях стоят номера выше написанных пунктов, а не их текст.

Теперь нужно передать значения роботу через радио модуль и неплохо было бы вывести их в терминал. Воспользуемся функциями sendData и printData :

Осталось только вызвать функции в loop :

Переходим к роботу.

Тут нам нужно будет написать алгоритм «дешифровки значений». Для его работы понадобится функция управления двигателями.

Вначале программы объявим пины на которые подключен драйвер двигателей (если вы таковой используете). Константно зададим границу скорости (середину максимального значения типа byte ). И напишем функцию управления двигателями.

Она принимает на вход 4 параметра:

  • направление левого двигателя,
  • его скорость,
  • направление правого двигателя,
  • его скорость.

1 – вращение вперед, 0 – назад. Скорость в диапазоне от 0 до 255. Как известно, ток течет от высокого потенциала к низкому, поэтому если подать на первый пин высокий сигнал, а на второй низкий, ток пойдет от первого ко второму и двигатель начнет вращаться в соответствующую сторону. Вместо постоянного высокого сигнала можно отправлять ШИМ сигнал, тогда мы сможем контролировать еще и скорость вращения, так и сделаем. Если принятое направление 1 (вперед), то на первый пин подадим низкий сигнал, а на второй ШИМ-ом принятую скорость, если направление 0, то наоборот. Для другого двигателя аналогично. Пропишем это в функции Motor.

Теперь можно писать сам алгоритм «дешифровки».

Проверяем, если значение скорости, лежащее в массиве data меньше либо равно установленной нами границе speed_border , то в функцию Motor передаем направление 0 (вращение назад) и само значение скорости. Если же значение больше чем граница, то передаем в Motor направление 1 (вперед), а из значения скорости вычитаем границу (это будет эквивалентно нахождению остатка от деления скорости на границу).

Этот алгоритм одинаков для обоих двигателей. Однако мы не можем передавать в функцию значения для двигателей по отдельности, для вызова функции мы должны сразу знать направления и скорости для обоих двигателей. Поэтому придется проверять условие сначала для левого, и если оно верно, то для правого и, если оно тоже верно, вызывать функцию Motor . Напишем это в функции drive . Сейчас скорость лежит в диапазоне от 0 до 128, а ШИМ сигнал имеет диапазон 0-255, совпадающий с диапазоном byte (именно поэтому использовался этот тип данных, ну еще он самый меньший из стандартных после bool , который нам не подходил). Поэтому скорость нужно умножать на 2. Предлагаю умножать значения скорости на 2 в функции driveHigh , а в drive передавать их без изменений, потом это может пригодиться.

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

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

Осталось лишь вызвать все функции в основном цикле loop .

Напоследок еще перспектива оптимизации. У каждого двигателя есть минимальные значения тока и напряжения, при которых он сможет работать, то есть двигатель робота будет вращаться только тогда, когда значения ШИМа превышают какой-то порог значений, назовем его «мертвым» интервалом. Этот порог в наибольшей степени зависит от мощности источника питания и самого двигателя, обычно он имеет значение от 30 до 70.

Поскольку двигатель в промежутке этих значений не работает, можно ими воспользоваться для кодирования чего-либо еще. При самом малом пороге в 30, мы получим 30 позиций кодировки с каждого двигателя, при комбинировании позиций с 2-х двигателей уже 900! Если от вашего робота не требуется мгновенная скорость отклика и количество отправляемых пакетов велико (>300-500), то можно потратить до 20% пакетов на отправку дополнительных кодов в «мертвых» интервалах, и вы этого не заметите.

Чем больше пакетов за 1 секунду получает робот, тем большее их количество в процентном соотношении можно потратить на отправку кодов, но чем больше их теряется, тем больший процент стоит оставлять для пакетов скорости, если вы хотите своевременного отклика. Для вылавливания этих пакетов нужно будет просто проверить, что значения скорости меньше заданной вами границы, а как обрабатывать их дальше (через математические вычисления, if или switch-case конструкции) и для чего применять подумайте сами.

Используя описанную в данной статье идею разбиения больших диапазонов на интервалы можно закодировать обе скорости для двигателей и еще какие-либо значения в одной переменной типа int , но нужно помнить, что одним пакетом мы можем передать столько кодов, сколько он содержал элементов массива, то есть при использовании массива из 2 элементов мы получали обе скорости за 1 пакет, а при использовании одной переменной нам пришлось бы использовать уже 2 пакета данный.

Учимся управлять серводвигателями через джойстики при помощи Arduino

Процесс сборки самоделки:

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

1. На модуле джойстика можно найти выходы U/R+ и L/R+. Через эти выходы происходит подключение питания. Соответственно сюда нужно подать напряжение +5V от соответствующего пина на Arduino.

2. Еще на джойстике присутствует два разъема под названием L/R и два разъема U/D. Их нужно подключить к аналоговым выходам А3 и А4.

3. Ну и в заключении землю на джойстике нужно соединить с землей на Arduino.

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

Шаг третий. Скетч для Arduino
Код очень простой и в нем присутствуют подробные комментарии. Приведенный код нужно просто скопировать в Arduino IDE. После того как код будет загружен, двигатели не должны двигаться. Они должны начинать двигаться только при нажатии кнопки на джойстике.

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

2. Бывает такое, что сразу после загрузки кода двигатели начинают вибрировать. Такое бывает если неправильно подключить пины U/D+ L/R+. Подключение нужно тщательно проверить. Чтобы не сжечь плату во время проверки, ее нужно обязательно отключить от компьютера.

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

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

Управление самобалансирующим роботом EduMip с помощью джойстика PS4 dualshock 4 через ROS

Это простой пример про то, как с помощью ROS можно связать несколько устройств по сети и пересылать данные управления.

Под катом в конце — видеодемонстрация управления роботом с джойстика.

Нам понадобится сам джойстик, можно взять от своей ps4 или купить в магазине. Я купил dualshock 4 v2, первой версии тоже должен работать.

Также нужен блютус на вашем компьютере с ROS, в документации к драйверам рекомендуют usb blutooth адаптеры версии 2.1+. У меня прекрасно заработал со встроенным в ноутбук адаптером. Также можно подключить джойстик и просто usb кабелем.

Затем в Ubuntu c ROS нужно установить драйвер по инструкции.

После установки драйверов и соединения джойстика по блютус, запускаем графическую утилиту jstest-gtk и видим что у нас два джойстика, нам нужен второй, так как на нем есть все кнопки и стики.

Так как по умолчанию ROS работает с joy0, то нам это надо переопределить на joy1:

Теперь нам нужно соединить это все в единую сеть. Ноутбук с ROS будет master, а EduMip с BeagleBone Blue ROS соединяться по wifi к ноутбуку. Оба устройства находятся в локальной сети wifi роутера.

На вашем ПК установите переменные среды ROS для поиска мастера ros (roscore) на ПК с команд .bashrc (добавьте эти команды в конец вашего файла .bashrc ):

На вашем EduMIP установите переменные среды ROS чтобы он нашел мастер ros (roscore) на ПК с команд .bashrc (добавьте эти команды в конец вашего файла .bashrc ):

Осталось только написать программу которая будет читать положение стика с джойстика и отправлять в топик, а робот этот топик читать.

На вашем ПК вы можете визуально посмотреть nodes и topics, запустив «rqt_graph»:

Демонстрация управления роботом с помощью джойстика:

Резюме: С помощью ROS можно построить систему для работа распределенную на несколько устройств и например большие вычисления производить на основном компьютере, на самом же роботе оставить только то что нужно для быстрого реагирования. На BeagleBone Blue стоит программируемая подсистеме реального времени (PRU‐ICSS).

PRU-ICSS состоит из микропроцессора на двух 32-битных ядрах, имеющих RISC-архитектуру и работающих на частоте 200МГц. Каждое ядро имеет свою область памяти, а также совместную с Linux область памяти, может использовать выводы общего назначения, расположенные на разъемах P8-P9, и формировать прерывания.

Кроме этого установленный на BeagleBone Blue датчик MPU9250 при изменении положения может формировать прерывание на которой можно повесить свою функцию, так собственно и сделано в EduMip.

Если говорить про следующий шаг эволюции то это ROS2, где одним из отличий является DDS (Data Distribution Service) позволяющий построить сеть разных взаимодействующих роботов и устройств на которых ROS не запущен.

Разработка роботов

Управление джойстиком с Qt

Иногда встает необходимость управлять роботом или другими устройствами удаленно. Я хочу обсудить дистанционное управлении с помощью джойстика, когда обработчик команд висит на компьютере и команды передаются на робота через канал передачи данных или команды обрабатываются непосредственно на самом устройстве, если он PC совместимо. Очевидные плюсы – дешевая цена джойстика от 300 руб. Удобно – джойстик зарекомендовал себя в играх.

Я написал небольшой класс VJoystickAdapter на С++ для Qt. Он взаимодействует с SDL дает пользователю удобный интерфейс работы с устройством и генерирует Qt сигналы при изменения состояния кнопок, аналоговых осей или других элементов джойстика. Связка SDL Qt делает код переносимым между разными OS. Рассмотрим UML диаграмму нашего класса.

Для подсоединения к джойстику используется метод open(int id) принимающий в качестве параметра id — идентификатор джойстика. В классе есть два вспомогательных статических метода getNumAvaliableJoystick() возвращающий число доступных в системе джойстиков и getAvaliableJoystickName() возвращающий список строк имен доступных в системе джойстиков. Для закрытия джойстика используется метод close() при вызове деструктора происходит проверка на закрытие джойстика и если вы забыли это сделаеть — это будет выполнено автоматически. Как можно заметить что внутри основного класса VJoystickAdapter описан класс VJoystickThread в котором запускается отдельный поток, класс VJoystickThread принимает указатель на базовый класс VJoystickAdapter и генерирует сигналы из метода run(). Генерируются следущие сигналы:

Названия сигналов говорят сами за себя: сигнал изменения состояния кнопки(Button), аналоговой оси(Axis), перекрестья(hat), и шарика(ball). Все сигналы передают два(три) параметра: id и состояние. Также в классе VJoystickAdapter есть методы для определения числа кнопок, аналоговых осей и других частей джойстика, метод возвращающий id и имя текущего джойстика. Значения возвращаемых параметров аналогичны тем что используются в SDL. Для аналоговых осей возвращаемая величина изменяется от -32768 до +32768. Значение кнокок — нажат, ненажат. Значения с перекрестья могут принимать значения из перечисления HatPosition:

Для Ball сказать ничего не могу — ибо у меня на джойстике нет такой штуки и в документации к SDL об этом ничего не сказано. Наверное по классу все.

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


Вот такое применение.Хочется отметить что за основу класс использует библиотеку SDL в текущей стабильной версии на момент написания статьи 1.2.14 отсутствует поддержка вибрации и обратной связи. Что немного печалит игроманов, но в целом на управлении не сказывается. Так как SDL портирована на множество операционных систем, то класс можно использовать везде где есть Qt и SDL. Точно могу сказать, что в Linux, Windows, MacOS этот код будет работать, все что нужно для работы его в других ОС это наличие SDL и Qt.
В прилагаемом архиве класс тестировался в Linux. Все что вам будет необходимо если вы используете не linux это подредактировать makefile.

В скором веремени планируется создать обработчик команд для управления роботом TurboT. Следите за темой .

Источники:

http://usamodelkina.ru/6830-uchimsya-upravlyat-servodvigatelyami-cherez-dzhoystiki-pri-pomoschi-arduino.html

http://habr.com/post/413901/

http://robot-develop.org/archives/1537

http://videonews.club/video/z29B4K_KNbx/uroki-Arduino-avtomaticheskaya-mashina-po-narezke-provoloki.html

Ссылка на основную публикацию