Как запрограммировать робота на ардуино

Подключение и управление манипулятором

Робот-платформа «Манипулятор» — позволяет собрать манипулятор для захвата и перемещения небольших предметов. В набор входит подробная инструкция по его сборке и наладке, а также пример скетча для управления манипулятором при помощи не входящих в состав набора, одной Arduino UNO, одного Trerma-Power Shield и четырёх Trema-потенциометров.

Видео:

• Сервоприводы подключаются к выводам D10-D7 на Trerma-Power Shield (можно менять в скетче).
• Trema-потенциометры подключаются к выводам A2-A5 на Trerma-Power Shield (можно менять в скетче).
• Джампер (перемычка) выбора схемы питания на плате Trerma-Power Shield устанавливается в положение «Общ.Vin».
• Источник питания на 7 — 12 В постоянного тока подключается к клеммнику на Trerma-Power Shield.
• Подключать питание к Arduino UNO не нужно, так как на Trerma-Power Shield установлен джампер «Общ.Vin», значит, на вход Vin Arduino UNO подается питание со входа «VinS» Trerma-Power Shield.

Питание:

У Trerma-Power Shield есть возможность выбора схемы питания установкой джампера (перемычки) на его плате в одно из двух положений: «Общ.Vin» или «Общ.5V». Манипулятор будет работать при любой схеме подключения питания:

• Если у Вас имеется источник питания на 7 — 12 В с проводом на конце которого имеется штекер, то его нужно подключить к Arduino UNO и установить джампер в положение «Общ.Vin».
• Если у Вас имеется источник питания на 7 — 12 В с проводом без штекера, то его нужно подключить к коннектору Trerma-Power Shield и установить джампер в положение «Общ.Vin».
• Если у Вас имеется источник питания на 7 — 30 В с проводом без штекера, то его нужно подключить к коннектору Trerma-Power Shield и установить джампер в положение «Общ.5V».
• Если Вы собираетесь манипулировать относительно тяжёлыми грузами, или используете более мощные сервоприводы (не из набора), то снимите джампер и подключите два источника питания (7 — 30 В к Trerma-Power Shield, а 7 — 12 В к Arduino UNO).

В приведённом выше примере подключения, используется вторая схема выбора питания: источник питания на 7 — 9 В подключён к клеммнику Trerma-Power Shield, а джампер (перемычка) установлен в положение «Общ.Vin».

Работа манипулятора:

Алгоритм программы (скетча) прост: поворот ручки Trema-потенциометра приводит в движение сервопривод.

• Поворот ручки первого Trema-потенциометра приведёт к повороту основания.
• Поворот ручки второго Trema-потенциометра приведёт к повороту левого плеча.
• Поворот ручки третьего Trema-потенциометра приведёт к повороту правого плеча.
• Поворот ручки четвёртого Trema-потенциометра приведёт в движение захват.

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

Код программы:

Если вы подадите питание, до калибровки, манипулятор может начать двигаться неадекватно! Сначала выполните все шаги калибровки.

Простой ИК-бот на Arduino Uno и Iskra JS

Простейший полноприводной бот, контролируемый с инфракрасного пульта дистанционного управления. Мы собрали двух роботов — на базе ARduino Uno и Iskra JS — и устроили гонку.

Что понадобится

Пульт дистанционного управления или любой пульт от бытовой техники.
Двусторонний скотч или клей

Видеообзор

Как собрать

Возьмите Troyka Shield LP и установите сверху на управляющую плату — Arduino или Iskra JS.

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

Подключите передние колёса «ServoFordL» и «ServoFordR» через 3-проводные шлейфы к 8 и 9 пину Troyka Shield LP соответственно, а задние колёса «ServoBackL» и «ServoBackR» — к 10 и 11 пину Troyka Shield LP соответственно.

Подключите ИК-приёмник к 4 пину Troyka Shield LP. В итоге должна получиться схема.

Установите Power Shield сверху на Troyka Shield LP.

Исходный код

Генерировать управляющие серводвигателями импульсы можно самостоятельно, но это настолько распространённая задача, что для её решения предусмотрена библиотека Servo . Она входит в установочный пакет Arduino IDE и доступна из Espruino Web IDE.

Библиотека Servo для Arduino
Библиотека Servo для Iskra JS

Скетч для Arduino

// библиотека для ИК приёмника #include // подключаем библиотеку для работы с сервоприводами #include // создаём объект для управления сервоприводом Servo myservoFordL; Servo myservoFordR; Servo myservoBackL; Servo myservoBackR; // даем имя пину подключения ИК приёмника int RECV_PIN = 4; // указываем к какому пину подключен ИК приёмник IRrecv irrecv(RECV_PIN); decode_results results; void setup() { // запускаем работу ИК приемника irrecv.enableIRIn(); // подключаем сервоприводы myservoFordL.attach(8); myservoFordR.attach(9); myservoBackL.attach(10); myservoBackR.attach(11); } void loop() { // принимаем данные с ИК пульта // в зависимости от нажатой кнопки пульта // даём разные команды роботу if (irrecv.decode(&results)) { if (results.value == 0x1689609F) { ford(); } else if (results.value == 0x1689B847) { back(); } else if (results.value == 0x168910EF) { left(); } else if (results.value == 0x16899867) { right(); } else if (results.value == 0x168938C7) { stop(); } // ждем следующее значение irrecv.resume(); } //делаем задержку delay(100); } // функция движение вперёд void ford() { myservoFordL.write(0); myservoFordR.write(180); myservoBackL.write(0); myservoBackR.write(180); } // функция движение назад void back() { myservoFordL.write(180); myservoFordR.write(0); myservoBackL.write(180); myservoBackR.write(0); } // функция поворота налево void left() { myservoFordL.write(180); myservoFordR.write(180); myservoBackL.write(180); myservoBackR.write(180); } // функция поворота направо void right() { myservoFordL.write(0); myservoFordR.write(0); myservoBackL.write(0); myservoBackR.write(0); } // функция остановки void stop() { myservoFordL.write(90); myservoFordR.write(90); myservoBackL.write(90); myservoBackR.write(90); }

Скрипт для Iskra JS

// подключаем модуль ИК-приёмника var receiver = require('@amperka/ir-receiver').connect(P4); // подключаем модули сервоприводов var myServoFordL = require('@amperka/servo').connect(P8); var myServoFordR = require('@amperka/servo').connect(P9); var myServoBackL = require('@amperka/servo').connect(P10); var myServoBackR = require('@amperka/servo').connect(P11); // если пришёл сигнал с ИК-пульта receiver.on('receive', function(code) { // в зависимости от нажатой кнопки пульта // даём разные команды роботу if (code === 0x1689609F) { ford(); } else if (code === 0x1689B847){ back(); } else if (code === 0x168910EF) { left(); } else if (code === 0x16899867) { right(); } else if (code === 0x168938C7) { stop(); } }); // функция движение вперёд function ford() { myServoFordL.write(180); myServoFordR.write(0); myServoBackL.write(180); myServoBackR.write(0); } // функция движение назад function back() { myServoFordL.write(0); myServoFordR.write(180); myServoBackL.write(0); myServoBackR.write(180); } // функция поворота налево function left() { myServoFordL.write(0); myServoFordR.write(0); myServoBackL.write(0); myServoBackR.write(0); } // функция поворота направо function right() { myServoFordL.write(180); myServoFordR.write(180); myServoBackL.write(180); myServoBackR.write(180); } // функция остановки function stop() { myServoFordL.write(90); myServoFordR.write(90); myServoBackL.write(90); myServoBackR.write(90); }

Что дальше

Научиться создавать сложных роботов поможет электронный конструктор «Робоняша».

Если вы прошли «Йодо» и хотите использовать его детали для сборки Робоняши, не обязательно покупать весь набор — мы подготовили подборку компонентов, необходимых для апгрейда первого эпизода до мобильного робота.

Если не указано иное, содержимое этой вики предоставляется на условиях следующей лицензии: CC Attribution-Noncommercial-Share Alike 4.0 International

Производные работы должны содержать ссылку на http://wiki.amperka.ru, как на первоисточник, непосредственно перед содержимым работы.
Вики работает на суперском движке DokuWiki.

робототехника/ir-servo-robot.txt · Последние изменения: 2023/11/06 16:25 — nikitos

Инструменты страницы

• Показать исходный текст
• История страницы
• Ссылки сюда
• Наверх

Урок 33. Обучаем Arduino робота ездить по линии

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

Видео:

Алгоритм работы:

• Если центральный датчик находится на линии, а боковые вне линии, то машинка едет прямо.
• Если левый датчик находится на линии, а правый вне линии, то машинка поворачивает налево (независимо от показаний центрального датчика)
• Если правый датчик находится на линии, а левый вне линии, то машинка поворачивает направо (независимо от показаний центрального датчика)
• Если правый и левый датчики находятся на линии (вне зависимости от показаний центрального датчика), то такое состояние является неопределённым, машинка продолжает предыдущее движение (прямо или с поворотом) в течении 2 секунд (можно менять в скетче). Если в течении этого времени состояние не изменится, то она остановится.
• Если все три датчика находятся вне линии, то такое состояние является неопределённым (потеря линии). Если этому состоянию предшествовал поворот, то машинка продолжит поворот в течении 2 секунд (можно менять в скетче). Если в течении этого времени состояние не изменится, то она остановится. Если этому состоянию не предшествовал поворот (машинка ехала прямо и линия оборвалась), то машинка сразу остановится.
• Если перед машинкой появилось препятствие, на расстоянии менее 10 см (можно менять в скетче), то машинка остановится и продолжит движение, как только препятствие исчезнет.

Скорость движения задаётся в константе valSpeed, от 1 до 255. Крутизна поворотов задаётся в константе valTurning, от 1 до 255. Время продолжения движения, при неопределённом состоянии, задаётся в константе tmrDelay, в микросекундах. Направление движения моторов указывается логическими значениями элементов массива arrRoute (0 элемент — правый мотор, 1 элемент — левый мотор), по умолчанию все элементы равны «1». Если вы перепутали полярность при подключении мотора, то измените значение соответствующего элемента этого массива на «0».

Калибровка для светлых, слабоконтрастных или цветных линий:

Машинка настроена на движение по темной линии, но она может ездить по светлым, слабоконтрастным или цветным линиям. Для этого её нужно откалибровать, указав значения для констант valSensor1 (показание датчика находящегося на линии) и valSensor0 (показание датчика находящегося вне линии). Для чего, в коде setup скетча, предусмотрен вывод показаний центрального датчика в монитор последовательного порта.

• Поместите машинку так, чтобы центральный датчик находился над линией.
• Подключите Arduino Uno по USB кабелю.
• Откройте монитор последовательного порта. В мониторе высветится показание датчика на линии.
• Поместите машинку так, чтобы центральный датчик находился вне линии.
• Нажмите кнопку reset на Motor Shield. В мониторе высветится показание датчика вне линии.
• Укажите первое значение константе valSensor1, а второе значение константе valSensor0 и повторно загрузите скетч.

Код программы:

// БИБЛИОТЕКИ: #include // подключаем библиотеку для работы с датчиком // НОМЕРА ВЫВОДОВ: const uint8_t pinSensorL = A3; // Вывод к которому подключен датчик находящийся слева (по направлению движения) const uint8_t pinSensorC = A4; // Вывод к которому подключен датчик находящийся по центру (по направлению движения) const uint8_t pinSensorR = A5; // Вывод к которому подключен датчик находящийся справа (по направлению движения) const uint8_t pinSens_TRIG = 2; // Вывод к которому подключен датчик расстояния HC_SR04 (вывод обозначенный на датчике как TRIG) const uint8_t pinSens_ECHO = 3; // Вывод к которому подключен датчик расстояния HC_SR04 (вывод обозначенный на датчике как ECHO) const uint8_t pinShield_LH = 7; // Вывод направления к которому подключен левый мотор (по направлению движения) const uint8_t pinShield_LE = 6; // Вывод ШИМ к которому подключен левый мотор (по направлению движения) const uint8_t pinShield_RE = 5; // Вывод ШИМ к которому подключен левый мотор (по направлению движения) const uint8_t pinShield_RH = 4; // Вывод направления к которому подключен левый мотор (по направлению движения) // ОБЪЕКТЫ: iarduino_HC_SR04_int hcsr(pinSens_TRIG, pinSens_ECHO); // Объект hcsr для работы с библиотекой iarduino_HC_SR04 (вывод TRIG, вывод ECHO) // УСТАНАВЛИВАЕМЫЕ ЗНАЧЕНИЯ: const uint16_t valSensor1 = 930; // Показание датчика находящегося на линии (указывается для конкретной трассы) const uint16_t valSensor0 = 730; // Показание датчика находящегося вне линии (указывается для конкретной трассы) const uint8_t valSpeed = 255; // Максимальная скорость (число от 1 до 255) const uint32_t tmrDelay = 2000; // Время в течении которого требуется остановиться (если в течении этого времени состояние остаётся неопределённым (линия не обнаружена), то требуется остановиться) const uint8_t valTurning = 10; // Крутизна поворотов (скорость реакции) (число от 1 до 255) const uint8_t valDistance = 20; // Минимально допустимое расстояние до объекта в сантиметрах (если расстояние будет меньше, то требуется остановитьтся) const bool arrRoute[2] = ; // Направление движения для каждого мотора (зависит от полярности, нулевой элемент - правый мотор, первый элемент - левый мотор) // РАССЧИТЫВАЕМЫЕ ЗНАЧЕНИЯ: uint8_t arrSpeed[2]; // Рассчитываемая скорость для каждого мотора (число от 1 до valSpeed, нулевой элемент - правый мотор, первый элемент - левый мотор) uint16_t valSensorM; // Рассчитываемое среднее значение датчика (значение между цветом линии и цветом вне линии) uint8_t valSensor; // Биты рассчитываемых логических уровней всех датчиков (0bxxxxxLCR) bool flgLine; // Флаг указывающий на то, что используется светлая линия (0 - тёмная линия, 1 - светлая линия) int8_t flgTurning; // Флаг наличия и направления поворота (0 - не поворачиваем, -1 - поворачиваем налево, +1 - поворачиваем направо) bool flgPWM; // Флаг указывающий на то, что требуется изменить ШИМ моторов (0 - тёмная линия, 1 - светлая линия) bool flgStop; // Флаг указывающий на необходимость остановиться (0 - без остановки, 1 - требуется остановиться) bool flgDistance; // Флаг обнаружения препятствия (0 - не обнаружено, 1 - обнаружено) uint32_t tmrMillis; // Время совершения последней операции (в миллисекундах) void setup()< // Узнаём цвет линии используемой на трассе, если он светлый, то устанавливаем флаг lineColor тёмный flgLine = (valSensor0>valSensor1); // Если условие (valSensor0>valSensor1) выполняется значит линия светлая и флаг flgLine установится в 1, иначе он сбросится в 0 // Вычисляем среднее значение между показаниями датчиков на линии и все линии if(flgLine) // Если на трассе используется светлая линия else // Если на трассе используется тёмная линия // Устанавливаем значение скорости обоих моторов arrSpeed[1]=valSpeed; // Максимальная скорость на левом моторе arrSpeed[0]=valSpeed; // Максимальная скорость на правом моторе // Устанавливаем флаг ШИМ, сбрасываем флаг наличия поворота, флаг остановки и флаг обнаружения припятствий flgPWM=1; flgTurning=0; flgStop=0; flgDistance=0; // Устанавливаем режим работы выводов и направление обоих моторов pinMode (pinSensorL, INPUT ); // Конфигурируем вывод pinSensorL как вход (для получения данных от левого датчика линии) pinMode (pinSensorC, INPUT ); // Конфигурируем вывод pinSensorC как вход (для получения данных от центрального датчика линии) pinMode (pinSensorR, INPUT ); // Конфигурируем вывод pinSensorR как вход (для получения данных от правого датчика линии) pinMode (pinShield_LH, OUTPUT ); // Конфигурируем вывод pinShield_LH как выход (для управления направлением движения левого мотора) pinMode (pinShield_LE, OUTPUT ); // Конфигурируем вывод pinShield_LE как выход (для управления скоростью вращения левого мотора, при помощи ШИМ) pinMode (pinShield_RE, OUTPUT ); // Конфигурируем вывод pinShield_RE как выход (для управления скоростью вращения правого мотора, при помощи ШИМ) pinMode (pinShield_RH, OUTPUT ); // Конфигурируем вывод pinShield_RH как выход (для управления направлением движения правого мотора) digitalWrite(pinShield_LH, arrRoute[1]); // Устанавливаем на выходе pinShield_LH уровень arrRoute[1] (направление движения левого мотора) digitalWrite(pinShield_RH, arrRoute[0]); // Устанавливаем на выходе pinShield_RH уровень arrRoute[0] (направление движения правого мотора) // Выводим показания центрального датчика линии Serial.begin(9600); while(!Serial)<> // Инициируем передачу данных по последовательному порту (на скорости 9600 бит/сек) Serial.println(analogRead(pinSensorC)); // Выводим показания центрального датчика линии (для указания значений константам valSensor0 и valSensor1) // Устанавливаем задержку и обновляем время совершения последней операции delay(2000); tmrMillis = millis(); > void loop()< // Читаем показания датчиков и преобразуем их в логические уровни // (1 - датчик на линии, 0 - датчик вне линии) valSensor = 0; // сбрасываем все биты переменной valSensor valSensor |= ((analogRead(pinSensorL)>valSensorM)^flgLine)valSensorM)^flgLine)valSensorM)^flgLine)" вернет "1" если значение analogRead(номер_вывода) больше чем значение valSensorM, а значит датчик находится над объектом, который темнее чем значение valSensorM, // Результат возвращённый оператором сравнения ">" нам подходит если используется тёмная линия, но если используется светлая линия, то результат нужно инвертировать . // Оператор побитового XOR "^" выполнит эту инверсию, только если установлен флаг flgLine, указывающий о том, что используется светлая линия // Оператор побитового сдвига влево " if(tmrMillis>millis()) < tmrMillis=0;>// Избавляемся от переполнения millis(); if(tmrMillis+tmrDelay // Останавливаемся если линия потеряна на более чем tmrDelay мс if(hcsr.distance() // Останавливаемся если обнаружено препятствие else if(flgDistance) < tmrMillis=millis(); flgPWM=1; flgDistance=0; >// Продолжаем движение если препятствие исчезло // Устанавливаем ШИМ для моторов if(flgPWM)0) arrSpeed[0]=valSpeed; break; // Уменьшаем скорость левого мотора (поворачиваем налево) case 0: arrSpeed[1]=valSpeed; arrSpeed[0]=valSpeed; break; // Устанавливаем одинаковую скорость (едим прямо) case 1: arrSpeed[1]=valSpeed; if(arrSpeed[0]>0) break; // Уменьшаем скорость правого мотора (поворачиваем направо) > if(flgStop) < arrSpeed[1]=0; arrSpeed[0]=0;>// Останавливаемся если установлен флаг flgStop if(flgDistance) < arrSpeed[1]=0; arrSpeed[0]=0;>// Останавливаемся если установлен флаг flgDistance // Выводим ШИМ analogWrite(pinShield_LE, arrSpeed[1]); analogWrite(pinShield_RE, arrSpeed[0]); > >

Ссылки:

• Код программы.
• Библиотека iarduino_HC_SR04_int.
• Wiki — Установка библиотек в Arduino IDE .
• Wiki — Датчик линии, аналоговый.
• Wiki — Ультразвуковой датчик расстояния HC-SR04 .
• Wiki — Trema Shield.

Урок 38. Дройдик (подключение, калибровка, первый запуск)

Процесс сборки подробно описан и проиллюстрирован в инструкции из робот-платформы «Дройдик».

Напряжение питания должно находится в пределах 7 . 12 В.

Калибровка:

Перед тем как учиться ходить, нужно научиться стоять.

Дело в том, что при сборке Дройдика, невозможно установить качалки сервоприводов с точностью до градуса. В результате смещения центральных положений ног и стоп, Дройдик будет косолапый и неуклюжий. Это исправляется с помощью калибровочного скетча, который позволяет «научить» Дройдика стоять. Калибровочный скетч позволяет менять угол каждого сервопривода отправкой цифр (от 1 до 8) через монитор последовательного порта. Таким образом можно выставить ноги Дройдика прямо и вертикально.

• Цифра 1 заставит левую ногу Дройдика повернуться влево, а цифра 2 — вправо, на 1°.
• Цифра 3 заставит правую ногу Дройдика повернуться влево, а цифра 4 — вправо, на 1°.
• Цифра 5 заставит левую ногу Дройдика наклониться влево, а цифра 6 — вправо, на 1°.
• Цифра 7 заставит правую ногу Дройдика наклониться влево, а цифра 8 — вправо, на 1°.
• Можно отправить сразу несколько цифр. Например, «11111», приведёт к повороту левой ноги влево, не на 1°, а на 5°.

После ввода цифры нужно нажать на Enter и в мониторе порта отобразятся текущие значения углов всех сервоприводов. Как только Вы выставили ноги Дройдика прямо и вертикально, перепишите последние значения из монитора порта. Эти значения и есть центральные углы сервоприводов в градусах, которые нужно присвоить четырём константам основного скетча: cenLeftTop, cenRightTop, cenLeftBot и cenRightBot (строки 12-15):

Строки 12-15 из основного скетча (значение констант требуется изменить):

. // Определяем константы: const uint8_t cenLeftTop = 100; // Определяем константу центрального угла в градусах для верхнего сервопривода левой ноги (по умолчанию = 100) const uint8_t cenRightTop = 80; // Определяем константу центрального угла в градусах для верхнего сервопривода правой ноги (по умолчанию = 80 ) const uint8_t cenLeftBot = 60; // Определяем константу центрального угла в градусах для нижнего сервопривода левой ноги (по умолчанию = 60 ) const uint8_t cenRightBot = 120; // Определяем константу центрального угла в градусах для нижнего сервопривода правой ноги (по умолчанию = 120) .

Новые значения (переписанные из монитора порта) вводятся вместо цифр: 100, 80, 60 и 120. После чего исправленный основной скетч можно загрузить в Arduino Uno, а калибровочный скетч уже будет не нужен.

Калибровочный скетч:

#include // Подключаем библиотеку для работы с сервоприводами // Определяем номера выводов: const uint8_t pinLeftTop = 4; // Значение должно совпадать со значением одноимённой константы основного скетча const uint8_t pinRightTop = 5; // Значение должно совпадать со значением одноимённой константы основного скетча const uint8_t pinLeftBot = 6; // Значение должно совпадать со значением одноимённой константы основного скетча const uint8_t pinRightBot = 7; // Значение должно совпадать со значением одноимённой константы основного скетча // Создаём переменные и объекты для сервоприводов uint8_t cenLeftTop=100, cenRightTop=80, cenLeftBot=60, cenRightBot=120; Servo serLeftTop , serRightTop , serLeftBot , serRightBot; void setup() < Serial.begin(9600); Serial.println(F("КАЛИБРОВОЧНЫЙ СКЕТЧ:\r\nвведите цифру от 1 до 8 и нажмите Enter . \r\nцифры 1 2 - поворачивают левую ногу на 1°\r\nцифры 3 4 - поворачивают правую ногу на 1°\r\nцифры 5 6 - наклоняют ступню левой ноги на 1°\r\nцифры 7 8 - наклоняют ступню правой ноги на 1°\r\nПример: - если ввсети 3 и нажать на Enter, то правая нога повернётся на 1°\r\n - если ввсети 3333333 и нажать на Enter, то та же нога повернётся на 7°\r\nУстановите ноги Дройдика прямо и вертикально, затем сохраните константы в основной скетч\r\n=====================================================================")); // Указываем объектам сервоприводов их выводы и устанавливаем сервоприводы в начальные положения: serLeftTop. attach(pinLeftTop); serLeftTop. write(cenLeftTop); serRightTop.attach(pinRightTop); serRightTop.write(cenRightTop); serLeftBot. attach(pinLeftBot); serLeftBot. write(cenLeftBot); serRightBot.attach(pinRightBot); serRightBot.write(cenRightBot); >void loop()< if(Serial.available()>0)< while(Serial.available())delay(10);> if(cenLeftTop >180) Serial.print(F("cenLeftTop = ")); Serial.print(cenLeftTop ); Serial.println(F(";\t\t// Верхний сервопривод левой ноги (поворот ноги осуществляется цифрами 1 или 2)")); serLeftTop. write(cenLeftTop ); if(cenRightTop>180) Serial.print(F("cenRightTop = ")); Serial.print(cenRightTop); Serial.println(F(";\t\t// Верхний сервопривод правой ноги (поворот ноги осуществляется цифрами 3 или 4)")); serRightTop.write(cenRightTop); if(cenLeftBot >180) Serial.print(F("cenLeftBot = ")); Serial.print(cenLeftBot ); Serial.println(F(";\t\t// Нижний сервопривод левой ноги (наклон ступни осуществляется цифрами 5 или 6)")); serLeftBot. write(cenLeftBot ); if(cenRightBot>180) Serial.print(F("cenRightBot = ")); Serial.print(cenRightBot); Serial.println(F(";\t\t// Нижний сервопривод правой ноги (наклон ступни осуществляется цифрами 7 или 8)")); serRightBot.write(cenRightBot); Serial.println("---------------------------------------------------------------------"); > >

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

Алгоритм работы Дройдика:

Движение прямо:

Для того что бы «научить» ходить любого робота, нужно подумать, а как ходите Вы? Если задать вопрос, как Вы поднимаете ногу во время движения, многие ответят — сгибаю колено. Но это верно лишь на половину. Вы не только незначительно сгибаете колено, но и незначительно наклоняетесь в противоположную сторону от той ноги которую поднимаете. У Дройдика нет колен, зато он может наклоняться (используя нижние сервоприводы), что и позволит ему поднимать левую или правую ногу. А поворачивая корпус верхним сервоприводом, стоя на одной ноге, он сможет продвигать вперёд или назад другую ногу.

Действия при движении:

При ходьбе каждая нога совершает 4 основных действия: поднимается, движется вперёд, опускается, движется назад. У человека первые три действия происходят быстрее и укладываются в то время пока вторая нога движется назад. Но мы упростим походку Дройдика так, что время совершения каждого действия будет одинаковым. Каждая нога будет совершать действие противоположное другой ноге: если левая нога продвигается вперёд — то правая нога продвигается назад, если левая нога опускается — то правая нога поднимается и т.д.

Повороты влево и вправо:

За повороты отвечает верхний сервопривод той ноги, которая находится наверху (свободна). Если верхний сервопривод свободной ноги, поворачивается на меньший угол чем верхний сервопривод опорной ноги (на которой стоит Дройдик) то он поворачивается в сторону опорной ноги. Чем больше разница углов, тем круче поворот.

Выбор направления:

При обнаружении препятствий, Дройдик должен повернуть, но как выбрать направление поворота? Обычно препятствия, например стены, появляются не перпендикулярно, а под углом. Дройдик, во время движения, вынужден поворачивать корпус влево и вправо для перемещения ног. Но вместе с корпусом поворачивается и ультразвуковой датчик расстояния. Получается что датчик измеряет расстояние не только прямо перед Дройдиком, а еще слева и справа. Таким образом, как только расстояние до препятствия сократилось до предельного, нужно выяснить, какая нога находится сзади, а какая спереди. Если спереди находится левая нога, значит и препятствие находится левее Дройдика, следовательно, нужно повернуть направо. И наоборот, если при обнаружении препятствия, спереди находится правая нога, значит нужно повернуть налево.

Код программы (основной скетч):

// Подключаем библиотеки: #include // Подключаем библиотеку iarduino_HC_SR04_int для работы с ультразвуковым датчиком HC-SR04+ #include // Подключаем библиотеку Servo для работы с сервоприводами // Определяем номера выводов: const uint8_t pinEcho = 2; // Определяем константу с номером вывода подключённым к выводу Echo датчика расстояний (можно указывать только те выводы Arduino, которые могут работать с внешними прерываниями) const uint8_t pinTrig = 3; // Определяем константу с номером вывода подключённым к выводу Trig датчика расстояний (может быть любым) const uint8_t pinLeftTop = 4; // Определяем константу с номером вывода подключённым к верхнему сервоприводу левой ноги (может быть любым) const uint8_t pinRightTop = 5; // Определяем константу с номером вывода подключённым к верхнему сервоприводу правой ноги (может быть любым) const uint8_t pinLeftBot = 6; // Определяем константу с номером вывода подключённым к нижнему сервоприводу левой ноги (может быть любым) const uint8_t pinRightBot = 7; // Определяем константу с номером вывода подключённым к нижнему сервоприводу правой ноги (может быть любым) // Определяем константы: const uint8_t cenLeftTop = 100; // Определяем константу центрального угла в градусах для верхнего сервопривода левой ноги (по умолчанию = 100) const uint8_t cenRightTop = 80; // Определяем константу центрального угла в градусах для верхнего сервопривода правой ноги (по умолчанию = 80 ) const uint8_t cenLeftBot = 60; // Определяем константу центрального угла в градусах для нижнего сервопривода левой ноги (по умолчанию = 60 ) const uint8_t cenRightBot = 120; // Определяем константу центрального угла в градусах для нижнего сервопривода правой ноги (по умолчанию = 120) const uint8_t maxStepSize = 15; // Определяем константу размера шага в градусах поворота верхних сервоприводов (чем больше угол, тем шире шаг) const uint8_t maxStepHeight = 10; // Определяем константу высоты шага в градусах наклона в стороны при ходьбе (чем больше угол, тем выше шаг) const uint8_t minDistance = 10; // Определяем константу минимального расстояния в см, при котором робот должен остановиться const uint32_t valDelay = 13; // Определяем константу обратно пропорциональную скорости движения (чем больше значение, тем медленнее скорость) // Создаём объекты: iarduino_HC_SR04_int sensor(pinTrig, pinEcho); // Создаём объект sensor для работы с датчиком расстояний, указывая номера выводов Trig и Echo Servo servoLeftTop; // Создаём объект servoLeftTop для работы с верхним левым сервоприводом Servo servoRightTop; // Создаём объект servoRightTop для работы с верхним правым сервоприводом Servo servoLeftBot; // Создаём объект servoLeftBot для работы с нижним левым сервоприводом Servo servoRightBot; // Создаём объект servoRightBot для работы с нижним правым сервоприводом // Создаём переменные: uint8_t valPosition = 224; // Определяем переменную (движение) для хранения текущей позиции шага (счёт от 0 до 255), начальная позиция 224 int8_t valTurning = 0; // Определяем переменную (поворот ) для пересчета размера шага в градусах поворота верхних сервоприводов (-10 - влево . 0 - прямо . +10 вправо) uint8_t maxLeftSize; // Объявляем переменную максимального размера шага в градусах поворота верхнего левого сервопривода (чем меньше угол, тем сильнее робот будет уходить влево ) uint8_t maxRightSize; // Объявляем переменную максимального размера шага в градусах поворота верхнего правого сервопривода (чем меньше угол, тем сильнее робот будет уходить вправо) // Создаём переменные для реализации алгоритма действий: uint8_t i = 0; // Счётчик шагов uint8_t f = 0; // Индикатор обнаружения препятствия: 0-нет препятствия, 1-препятствие появилось, 2-есть препятствие, 3-препятствие исчезло // // Если препятствие нет, то идём прямо. Если препятствие только появилось, то начинаем поворот. Если препятствие только исчезло, то делаем несколько шагов и начинаем идти прямо void setup() < // Указываем объектам сервоприводов их выводы: servoLeftTop. attach(pinLeftTop); // Указываем объекту servoLeftTop работать с выводом pinLeftTop servoRightTop.attach(pinRightTop); // Указываем объекту servoRightTop работать с выводом pinRightTop servoLeftBot. attach(pinLeftBot); // Указываем объекту servoLeftBot работать с выводом pinLeftBot servoRightBot.attach(pinRightBot); // Указываем объекту servoRightBot работать с выводом pinRightBot // Устанавливаем центральные углы сервоприводов: servoLeftTop. write (cenLeftTop); // Устанавливаем центральную позицию (угол cenLeftTop ) для сервопривода подключённого к выводу pinLeftTop servoRightTop.write (cenRightTop); // Устанавливаем центральную позицию (угол cenRightTop) для сервопривода подключённого к выводу pinRightTop servoLeftBot. write (cenLeftBot); // Устанавливаем центральную позицию (угол cenLeftBot ) для сервопривода подключённого к выводу pinLeftBot servoRightBot.write (cenRightBot); // Устанавливаем центральную позицию (угол cenRightBot) для сервопривода подключённого к выводу pinRightBot // Ждём адекватных ответов от датчика расстояний: delay(500); sensor.distance(); sensor.distance(); >void loop () < // ============================================== // РЕАЛИЗУЕМ АЛГОРИТМ ПОВЕДЕНИЯ ДРОЙДИКА: if(millis()%valDelay==0)< // Выполняем действие один раз в valDelay миллисекунд . valPosition++; // Приращаем значение переменной valPosition заставляя робота идти вперёд // valPosition--; // А так робот пойдёт назад if(!valPosition)// Считаем шаги в переменную i if(sensor.distance()<=minDistance)< // Если обнаружено препятствие (реальное расстояние до объекта возвращённое функцией sensor.distance() меньше чем указано в константе minDistance), то . if(f==0)else // Если препятствий не было (f==0), то устанавливаем индикатор i в значение 1 (препятствие появилось) if(f==1) // Если препятствие появилось (f==1), то устанавливаем индикатор i в значение 2 (препятствие есть) >else< // Если препятствий нет (реальное расстояние до объекта возвращённое функцией sensor.distance() больше чем указано в константе minDistance), то . if(f==1)else // Если препятствие появилось (f==1), то устанавливаем индикатор i в значение 3 (препятствие исчезло) и сбрасываем счётчик шагов if(f==2)else // Если препятствие было (f==2), то устанавливаем индикатор i в значение 3 (препятствие исчезло) и сбрасываем счётчик шагов if(f==3 && i>2) // Если препятствие исчезло (f==1) и робот совершил более 2 шагов в сторону (i>2), то сбрасываем индикатор i в значение 0 (препятствий нет) > if(f==0)else // Если препятствий нет (f==0), то не поворачиваем if(f==1)< // Если препятствие появилось (f==1), то . if(valPosition>127) // Если вперёд шла левая нога (valPosition>127), то начинаем поворачивать вправо (если требуется повернуть более мягко, то присваиваем переменной valTurning значение ниже 10 ) else // Если вперёд шла правая нога (valPosition <128), то начинаем поворачивать влево (если требуется повернуть более мягко, то присваиваем переменной valTurning значение выше -10 ) >> // ============================================== // Шагаем: Движение осуществляется в соответствии со значениями переменных: valPosition и valTurning // Если значение переменной valPosition приращается, то робот будет идти вперёд // Если значение переменной valPosition убывает, то робот будет идти назад // Чем быстрее выполняется приращение/убавление переменной valPosition, тем быстрее шагает робот // Значение переменной valTurning управляет поворотом робота, 0-прямо, 1-прямо и чуть правее, (чем выше значение - тем круче поворот), 10 поворот вправо на месте. // Отрицательные значения переменной valTurning действуют аналогично положительным, но поворот осуществляется влево. maxLeftSize =maxStepSize; if(valTurning <0)// Корректируем значение maxLeftSize в соответствии со значением valTurning maxRightSize=maxStepSize; if(valTurning>0) // Корректируем значение maxRightSize в соответствии со значением valTurning // Полный шаг каждой ноги разбит на 4 сектора в соответствии со значением переменной valPosition // Каждая нога последовательно выполняет определённое движение для каждого сектора: вверх, вперёд, вниз, назад. if(valPosition отходит назад servoRightTop.write(map(valPosition, 0, 63, cenRightTop - maxRightSize , cenRightTop + maxRightSize ));>else // Правая нога поворачивается вправо => выходит вперёд if(valPosition <128)переносит центр тяжести с себя на правую ногу, которая станет опорной servoRightBot.write(map(valPosition, 64, 127, cenRightBot -(maxStepHeight/2) , cenRightBot + maxStepHeight ));>else // Правая нога наклоняется вправо => опускается вниз (становится опорной) и поднимает левую ногу if(valPosition <192)выходит вперёд servoRightTop.write(map(valPosition, 128, 191, cenRightTop + maxRightSize , cenRightTop - maxRightSize ));>else // Правая нога поворачивается влево => отходит назад /*valPosition опускается вниз (становится опорной) и поднимает правую ногу servoRightBot.write(map(valPosition, 192, 255, cenRightBot + maxStepHeight , cenRightBot -(maxStepHeight/2)));> // Правая нога наклоняется влево => переносит центр тяжести с себя на левую ногу, которая станет опорной // ============================================== >

Ссылки:

• Код программ (основной скетч).
• Библиотека iarduino_HC_SR04.
• Калибровочный скетч.
• Wiki — Установка библиотек в Arduino IDE .
• Wiki — Ультразвуковой датчик расстояния HC-SR04.
• Wiki — Trema Shield.