КОРЗИНА
магазина
8 (499) 500-14-56 | ПН. - ПТ. 12:00-18:00
ЛЕСНОРЯДСКИЙ ПЕРЕУЛОК, 18С2, БЦ "ДМ-ПРЕСС"

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

Введение:

В этом уроке мы создадим Дройдика - прямоходящего робота, которого «научим» стоять, ходить и обходить препятствия.

Видео:

Нам понадобится:

Для реализации проекта нам необходимо установить библиотеку:

  • iarduino_HC_SR04_int для работы с ультразвуковыми датчиками расстояния.
  • Библиотека Servo используемая в скетче, входит в стандартный набор Arduino IDE.

О том как устанавливать библиотеки, Вы можете ознакомиться на странице Wiki - Установка библиотек в Arduino IDE

Схема подключения:

Сборка Дройдика

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

Напряжение питания должно находится в пределах 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      <Servo.h>         //  Подключаем библиотеку для работы с сервоприводами
//            Определяем номера выводов:
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()){switch(Serial.read()){
    case '1': cenLeftTop--; break; case '3': cenRightTop--; break; case '5': cenLeftBot--; break; case '7': cenRightBot--; break;
    case '2': cenLeftTop++; break; case '4': cenRightTop++; break; case '6': cenLeftBot++; break; case '8': cenRightBot++; break;
    }delay(10);}
    if(cenLeftTop >180){cenLeftTop =180;} Serial.print(F("cenLeftTop  = ")); Serial.print(cenLeftTop ); Serial.println(F(";\t\t//  Верхний сервопривод левой  ноги (поворот ноги  осуществляется цифрами 1 или 2)")); serLeftTop. write(cenLeftTop );
    if(cenRightTop>180){cenRightTop=180;} Serial.print(F("cenRightTop = ")); Serial.print(cenRightTop); Serial.println(F(";\t\t//  Верхний сервопривод правой ноги (поворот ноги  осуществляется цифрами 3 или 4)")); serRightTop.write(cenRightTop);
    if(cenLeftBot >180){cenLeftBot =180;} Serial.print(F("cenLeftBot  = ")); Serial.print(cenLeftBot ); Serial.println(F(";\t\t//  Нижний  сервопривод левой  ноги (наклон ступни осуществляется цифрами 5 или 6)")); serLeftBot. write(cenLeftBot );
    if(cenRightBot>180){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.h>     //  Подключаем библиотеку iarduino_HC_SR04_int для работы с ультразвуковым датчиком HC-SR04+
#include             <Servo.h>                    //  Подключаем библиотеку 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++;}                    //  Считаем шаги в переменную i
        if(sensor.distance()<=minDistance){       //  Если обнаружено препятствие (реальное расстояние до объекта возвращённое функцией sensor.distance() меньше чем указано в константе minDistance), то ...
            if(f==0){f=1;}else                    //  Если препятствий не было    (f==0), то устанавливаем индикатор i в значение 1 (препятствие появилось)
            if(f==1){f=2;}                        //  Если препятствие появилось  (f==1), то устанавливаем индикатор i в значение 2 (препятствие есть)
        }else{                                    //  Если препятствий нет        (реальное расстояние до объекта возвращённое функцией sensor.distance() больше чем указано в константе minDistance), то ...
            if(f==1){f=3; i=0;}else               //  Если препятствие появилось  (f==1), то устанавливаем индикатор i в значение 3 (препятствие исчезло) и сбрасываем счётчик шагов
            if(f==2){f=3; i=0;}else               //  Если препятствие было       (f==2), то устанавливаем индикатор i в значение 3 (препятствие исчезло) и сбрасываем счётчик шагов
            if(f==3 && i>2){f=0;}                 //  Если препятствие исчезло    (f==1) и робот совершил более 2 шагов в сторону (i>2), то сбрасываем индикатор i в значение 0 (препятствий нет)
        }
        if(f==0){valTurning=0;}else               //  Если препятствий нет        (f==0), то не поворачиваем
        if(f==1){                                 //  Если препятствие появилось  (f==1), то ...
            if(valPosition>127){valTurning= 10;}  //  Если вперёд шла левая  нога (valPosition>127), то начинаем поворачивать вправо (если требуется повернуть более мягко, то присваиваем переменной valTurning значение ниже  10 )
            else               {valTurning=-10;}  //  Если вперёд шла правая нога (valPosition<128), то начинаем поворачивать влево  (если требуется повернуть более мягко, то присваиваем переменной valTurning значение выше -10 )
        }
    }
//  ==============================================
//  Шагаем:             Движение осуществляется в соответствии со значениями переменных: valPosition и valTurning
//                      Если значение переменной valPosition приращается, то робот будет идти вперёд
//                      Если значение переменной valPosition убывает, то робот будет идти назад
//                      Чем быстрее выполняется приращение/убавление переменной valPosition, тем быстрее шагает робот
//                      Значение переменной valTurning управляет поворотом робота, 0-прямо, 1-прямо и чуть правее, (чем выше значение - тем круче поворот), 10 поворот вправо на месте.
//                      Отрицательные значения переменной valTurning действуют аналогично положительным, но поворот осуществляется влево.
                        maxLeftSize =maxStepSize; if(valTurning<0){maxLeftSize =map(valTurning, 0,-10, maxStepSize, 0);} // Корректируем значение maxLeftSize  в соответствии со значением valTurning
                        maxRightSize=maxStepSize; if(valTurning>0){maxRightSize=map(valTurning, 0, 10, maxStepSize, 0);} // Корректируем значение maxRightSize в соответствии со значением valTurning
//                      Полный шаг каждой ноги разбит на 4 сектора в соответствии со значением переменной valPosition
//                      Каждая нога последовательно выполняет определённое движение для каждого сектора: вверх, вперёд, вниз, назад.
    if(valPosition<64 ){servoLeftTop. write(map(valPosition,   0,  63, cenLeftTop  - maxLeftSize      , cenLeftTop  + maxLeftSize     ));      // Левая  нога поворачивается вправо => отходит    назад
                        servoRightTop.write(map(valPosition,   0,  63, cenRightTop - maxRightSize     , cenRightTop + maxRightSize    ));}else // Правая нога поворачивается вправо => выходит    вперёд
    if(valPosition<128){servoLeftBot. write(map(valPosition,  64, 127, cenLeftBot  - maxStepHeight    , cenLeftBot  +(maxStepHeight/2)));      // Левая  нога наклоняется    вправо => переносит  центр тяжести с себя на правую ногу, которая станет опорной
                        servoRightBot.write(map(valPosition,  64, 127, cenRightBot -(maxStepHeight/2) , cenRightBot + maxStepHeight   ));}else // Правая нога наклоняется    вправо => опускается вниз (становится опорной) и поднимает левую ногу
    if(valPosition<192){servoLeftTop. write(map(valPosition, 128, 191, cenLeftTop  + maxLeftSize      , cenLeftTop  - maxLeftSize     ));      // Левая  нога поворачивается влево  => выходит вперёд
                        servoRightTop.write(map(valPosition, 128, 191, cenRightTop + maxRightSize     , cenRightTop - maxRightSize    ));}else // Правая нога поворачивается влево  => отходит назад
    /*valPosition<255*/{servoLeftBot. write(map(valPosition, 192, 255, cenLeftBot  +(maxStepHeight/2) , cenLeftBot  - maxStepHeight   ));      // Левая  нога наклоняется    влево  => опускается вниз (становится опорной) и поднимает правую ногу
                        servoRightBot.write(map(valPosition, 192, 255, cenRightBot + maxStepHeight    , cenRightBot -(maxStepHeight/2)));}     // Правая нога наклоняется    влево  => переносит  центр тяжести с себя на левую ногу, которая станет опорной
//  ==============================================
}

Ссылки:




Обсуждение

Гарантии и возврат Используя сайт Вы соглашаетесь с условями