Инструкция по сборке манипулятора ардуино

Введение:

В данном уроке, двигаясь по шагам, мы соберём робот «Манипулятор».

Видео:

Для сборки нам понадобится крепеж:

Шаг 1

Список деталей к Шагу 1

Шаг 2

Список деталей к Шагу 2

Шаг 3

Список деталей к Шагу 3

Шаг 4

Список деталей к Шагу 4

Шаг 5

Список деталей к Шагу 5

Шаг 6

Список деталей к Шагу 6

Шаг 7

Список деталей к Шагу 7

Шаг 8

Список деталей к Шагу 8

Шаг 9

Список деталей к Шагу 9

Вы можете скачать данную инструкцию по ссылке: Сборка робота-манипулятора. Часть 1

В собранном виде:

Рука манипулятор — это потрясающее устройство! Во всем мире их используют заводы, они красят, припаивают и перемещают вещи. Их также можно увидеть в космических исследованиях, подводных машинах и даже в медицинских целях!

И теперь у Вас есть возможность собрать дешёвую версию в вашем собственном доме, офисе или лаборатории! Устали делать повторяющуюся работу? Программируйте свой манипулятор, чтобы манипулятор решал за Вас эти задачи….

В этом уроке мы покажем Вам, как собрать роботизированную руку и как ее запрограммировать с помощью Arduino Mega. Для этого проекта мы также испытаем другой метод управления манипулятором: используя контроллеры – Nintendo Nunchuk! Они дешевы, легкие в поиске и имеют множество датчиков.

Существует несколько способов использования этого урока. Если у вас нет набора для манипулятора (и Вы не хотите его покупать или создавать), Вы все равно можете прочитать урок, чтобы узнать что-то о программировании Arduino и о том, как связать контроллер Wii Nunchuk с Вашими собственными проектами. Урок так же поможет, в практике навыков сборки электроники.

Шаг 1. Инструменты и материалы

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

• Припой и проволока. Нам пришлось припаять несколько контактов к проводам контроллера, чтобы подключить их к Arduino;
• Термоусадочная трубка. Термоусадочная трубка использовались для лучшей изоляции проводников;
• Отвертка. Конструкция собирается с использованием болтов и гаек;
• 6-осевая механическая настольная роботизированная рука https://www.sainsmart.com/collections/robotics-cnc/products/6-axis-mechanical-desktop-robotic-arm Этот комплект уже поставляется с несколькими компонентами, (имеется бесплатная доставка по миру при стоимости более 50$);
• Источник питания 12 В (2А и более) – DC12V AC100-240V ;
• Контроллер Nunchuk ( Nunchuk ). Он взаимодействует с платой Arduino и используется для управления манипулятором;
• Перемычки Папа (4 провода);
• Arduino Mega (Ссылка ).Обратите внимание, что комплект манипулятора, который я предлагал выше, имеет комплект, которая уже поставляется с платой Arduino. Если вы не используете этот наборы, вы можете использовать другие платы Arduino;

Информируем, что есть адаптер для контроллера Nunchuk, который упрощает подключение к плате ( WiiChuck Nunchuck Adapter shield Module Board For Arduino) Это хороший вариант, если Вы хотите подключить контроллер и не хотите разрушать оригинальный разъем, как описано на шаге 9.

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

Шаг 2. Сборка Манипулятора ч.1 – Основание

Первая часть, которую мы соберём, будет – основа робота.

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

Шаг 3. Сборка Манипулятора ч.2 – Сервопривод #1

Первый серводвигатель монтируется перпендикулярно основанию с помощью сервопривода. Этот профиль крепится к основанию с помощью четырех болтов и гаек M3, как показано на рисунках. Серво № 1 находится на вершине и крепится с помощью четырех болтов и гаек M3.

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

Шаг 4. Сборка Манипулятора ч.3 – Сервопривод #2

Другой кронштейн сервопривода установлен перпендикулярно предыдущему. Он подключен к Серводвигателю №1 с использованием четырех болтов M3. Серво № 2 установлен с четырьмя болтами и гайками M3, а также используется круглый металлический рожок.

Затем кронштейн U крепится с помощью четырех болтов. Обратите внимание, что болт M3 используется опорой оси сервопривода. Это дает стабильность структуре. Подшипник подходит для этого болта, для фиксирования на месте, используйте другую гайку M3. Таким образом, кронштейн U плотно прикреплен к центральной оси сервопривода № 2.

Шаг 5. Сборка Манипулятора ч.4 – Сервопривод #3

Другой U-образный кронштейн монтируется с использованием четырех болтов M3 и гаек.

На другом конце установлен сервопривод №3 с использованием круглого металлического рожка и четырех болтов. Сервопривод подключен к сервомотору, а профили L-образной формы соединены с кронштейном сервопривода с помощью нескольких болтов и гаек.

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

Шаг 6. Сборка Манипулятора ч.5 – Сервопривод #4

Другой U-образный кронштейн соединен с профилем L-образной формы с помощью набора из четырех болтов M3 и гаек. Как и в предыдущем шаге, сервомеханизм № 4 монтируется к кронштейну U с использованием четырех болтов. К сервоусилителю подключен другой кронштейн.

Шаг 7. Сборка Манипулятора ч.6 – Сервопривод #5

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

Шаг 8. Сборка Манипулятора ч.7 – Сервопривод #6

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

Шаг 9. Подготовка контроллера Nunchuk

Для этого проекта мы решили использовать контроллер Nintendo Nunchuk по ряду причин:

1. Они дешевые! Реплики могут иметь низкое качество, но нам не нужен надежный контроллер для этого проекта;
2. Их легко найти! В Интернете есть несколько оригинальных и недорогих реплик.
3. В нем много датчиков! Каждый контроллер имеет две кнопки (кнопки Z и C), двухосный джойстик (X и Y) и трехосный акселерометр;
4. Для него есть библиотека на Arduino. Роберт Эйзеле разработал удивительную и удобную библиотеку для чтения сенсора Nunchuk.

К несчастью, джойстики Nunchuk имеют неудобный разъем, который трудно соединить с другой электроникой. Чтобы подключить его к Arduino, нам пришлось разрезать кабель и разделить провода. Таким образом, он больше не будет работать с Nintendo Wii …: /

Сначала нам пришлось отрезать разъем джойстика и снять изоляцию провода. Используя мультиметр и исходя из цвета каждого провода, я определил функцию каждого провода (Vcc, GND, SCL и SDA) на основе схемы разъема, показанного на рисунке. Цвет проводов не имеет определённого стандарта. Мы уже сталкивались со следующими конфигурациями:

Оригинал:

• SDA = зеленый
• SCL = желтый
• 3V3 = красный
• GND = белый

Реплика 1:

• SDA = желтый
• SCL = белый
• 3V3 = зеленый
• GND = красный

Реплика 2:

• SDA = blue
• SCL = white
• 3V3 = pink
• GND = green

Мы припаяли провода к перемычке (папа), чтобы легче было подключиться к плате Arduino. Для этого использовали паяльник и термоусадочную трубку, как показано на рисунках.

Позднее нам сообщили, что есть адаптер Nunchuk, который упрощает подключение к плате ( WiiChuck Nunchuck Adapter shield Module Board For Arduino). Это хороший вариант, если Вы хотите, сэкономить время на пайке и не хотите разрушать оригинальный разъем.

Шаг 10. Схема подключения

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

Подключите компоненты следующим образом:

Контроллер:

• Контроллер контакт 6 (SCL) => Arduino Mega Контакт 21 (SCL) (на плате)
• Контроллер контакт 1 (SDA) => Arduino Mega Контакт 20 (SDA) (на плате)
• Контроллер контакт 3 (Vcc) => Ardino Mega Контакт 3V3 (на плате)
• Контроллер контакт 4 (Gnd) => Arduino Mega Контакт Gnd (на плате)

Если вы используете Arduino Uno, контакты Nunchuk SCL и SDA должны быть подключены к контактам Arduino следующим образом:

• Контроллер контакт 6 (SCL) => Arduino Uno контакт A5
• Контроллер контакт 1 (SDA) => Arduino Uno контакт A4
• Контроллер контакт 3 (Vcc) => Ardino Uno контакт 3V3
• Контроллер контакт 4 (Gnd) => Arduino Uno контакт Gnd

Сервопривод:

• Контакт Платы управления 11 => Сервопривод № 1
• Контакт Платы управления 12 => Сервопривод №2
• Контакт Платы управления 13 => Сервопривод № 3
• Контакт Платы управления 8 => Сервопривод № 4
• Контакт Платы управления 9 => Сервопривод №5
• Контакт Платы управления 10 => Сервопривод №6

Если вы не используете плату управления, Вы должны использовать следующую конфигурацию контактов:

• Arduino Контакт 11 => Серво #1 (Sgn)
• Arduino Контакт 12 => Серво #2 (Sgn)
• Arduino Контакт 13 => Серво #3 (Sgn)
• Arduino Контакт 8 => Серво #4 (Sgn)
• Arduino Контакт 9 => Серво #5 (Sgn)
• Arduino Контакт 10 => Серво #6 (Sgn)
• Arduino Контакт Gnd => Серво Gnd
• 6В Контакт питания => Серво Vcc

Вам также необходимо подключить внешний источник питания 12 В. Мы предлогаем использовать один блок с выходом более 2A. Сервоприводы потребляют много энергии, и если блок питания недостаточно мощный, сервоприводы будут вибрировать и перегреваться. Они также потеряют свою мощность.

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

Подключите USB-кабель к Arduino и перейдите к следующему шагу.

Шаг 11 Настройка Arduino IDE

Теперь, когда оборудование готово, пришло время поработать над кодом Arduino.

1. Скачайте и установите новую версию Arduino IDE

Вы можете найти последнюю версию для Windows, Linux или MAC OSX на веб-сайте Arduino.

1. Добавление библиотек

Для этого проекта я использовал потрясающую библиотеку Nunchuk Arduino Роберта Эйзеля! Подробнее Вы можете узнать на его сайте:

Перейдите в Sketch-> Include Library -> Manage Libraries… на вашей Arduino IDE для добавления библиотеки.

Перейдите в Скетч-> Подключить Библиотеку -> Добавить Библиотеку… на вашей Arduino IDE для добавления библиотеки.

Как работает библиотека?

В библиотеке Nunchuk имеется набор функций для считывания датчиков контроллера:

nunchuk_buttonZ (): возвращает 1, если нажата кнопка Z, или 0, если это не так;

nunchuk_buttonC (): возвращает 1, если нажата кнопка C, или 0, если это не так;

nunchuk_joystickX (): возвращает значение x джойстика (от -127 до 127);

nunchuk_joystickY (): возвращает значение y джойстика (от -127 до 127);

nunchuk_pitch (): возвращает угол контроллера в радианах (от -180º до 180º);

nunchuk_roll (): возвращает угол наклона контроллера в радианах (от -180º до 180º).

Углы возвращаются в радианах. Мы преобразовали эти значения в градусы в коде Arduino.

Загрузите скетч файл Arduino.

Подключите USB-кабель к USB-порту вашего компьютера и загрузите код. Загрузка кода занимает время, Вам нужно проявить терпение.

После полной загрузки, отсоедините USB-кабель, подключите источник питания к Arduino и включите кнопку питания. Код начнет работать моментально.

Предупреждение: При запуске кода, манипулятор быстро переместится в исходное положение. Будьте осторожны, чтобы не пораниться или повредить оборудование во время запуска!

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

Объяснение кода:

Перед установкой кода импортируте библиотеки, используемые в эскизе (nunchuk.h, wire.h и servo.h).

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

Переменные servo # _speed определяют скорость движения каждого сервопривода. Если вы хотите, чтобы определенный сервопривод двигался быстрее, увеличьте его значение. Угол # min и угол # max используются для ограничения максимального и минимального угла для каждого сервопривода. Вы можете установить эти переменные, чтобы избежать коллизий между последовательными суставами робота.

//Include libraries
#include 
#include 
#include 

//define variables
#define SERV1 8    //servo 1 on digital port 8
#define SERV2 9    //servo 2 on digital port 9
#define SERV3 10   //servo 3 on digital port 10
#define SERV4 11   //servo 4 on digital port 11
#define SERV5 12   //servo 5 on digital port 12
#define SERV6 13   //servo 6 on digital port 13

Servo s1; //servo 1
Servo s2; //servo 2
Servo s3; //servo 3
Servo s4; //servo 4
Servo s5; //servo 5
Servo s6; //servo 6

//define starting angle for each servo
//choose a safe position to start from
//it will try to move instantaniously to that position when powered up!
//those angles will depend on the angle of each servo during the assemble
int angle1 = 90; //servo 1 current angle
int angle2 = 30; //servo 2 current angle
int angle3 = 0;  //servo 3 current angle
int angle4 = 90; //servo 4 current angle
int angle5 = 90; //servo 5 current angle
int angle6 = 45; //servo 6 current angle
int servo1_speed = 3; //servo 1 speed
int servo2_speed = 3; //servo 2 speed
int servo3_speed = 3; //servo 3 speed
int servo4_speed = 1; //servo 4 speed
int servo5_speed = 1; //servo 5 speed

//define restrictions for each servo
//those angles will depend on the angle of each servo during the assemble
int angle1min = 0;   //servo 1 minimum angle
int angle1max = 180; //servo 1 maximum angle
int angle2min = 0;   //servo 2 minimum angle
int angle2max = 180; //servo 2 maximum angle
int angle3min = 0;   //servo 3 minimum angle
int angle3max = 180; //servo 3 maximum angle
int angle4min = 0;   //servo 4 minimum angle
int angle4max = 180; //servo 4 maximum angle
int angle5min = 0;   //servo 5 minimum angle
int angle5max = 180; //servo 5 maximum angle
int angle6min = 0;   //servo 6 minimum angle
int angle6max = 180; //servo 6 maximum angle

boolean display_angles = true; //boolean used to update the angle of each servo on Serial Monitor

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

Здесь также запускается последовательная связь (с последовательным монитором) и связь I2C с Nunchuck.

//SETUP

void setup() {
    //attach each servo to a pin and start its position
    s1.attach(SERV1);
    s1.write(angle1);
    s2.attach(SERV2);
    s2.write(angle2);
    s3.attach(SERV3);
    s3.write(angle3);
    s4.attach(SERV4);
    s4.write(angle4);
    s5.attach(SERV5);
    s5.write(angle5);
    s6.attach(SERV6);
    s6.write(angle6);

    //start serial communication
    Serial.begin(9600);

    //start Nunchuk
    Wire.begin();
    nunchuk_init();
}

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

void loop() {

    //read Nunchuk sensors
    if (nunchuk_read()) {        
        int x = nunchuk_joystickX();    //joystick X position
        int y = nunchuk_joystickY();    //joystick Y position
        boolean z = nunchuk_buttonZ();  //z button status
        boolean c = nunchuk_buttonC();  //c button status
        float pitch = nunchuk_pitch();  //pitch angle
        float roll = nunchuk_roll();    //roll angle

Джойстик X будет использоваться для перемещения серво #1.

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

//Turn left/right (at a fixed speed)
        //Turn left
        if (x > 90) {
          angle1 -= servo1_speed;
          display_angles = true;
          if (angle1 < angle1min) {
            angle1 = angle1min;
          }
        }
        //Turn right
        if (x < -90) {
          angle1 += servo1_speed;
          display_angles = true;
          if (angle1 > angle1max) {
            angle1 = angle1max;
          }
        }
        s1.write(angle1); //update servo position

Аналогичный блок используется для джойстика Y. Он используется для изменения угла Серво #3. Серво #2 сохраняется в этом коде.

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

Когда нажимается только кнопка C, код считывает угол поворота и использует его как заданное значение. Серво #5 вращается до достижения заданного значения. Это скорость пропорциональна ошибке между фактическим и желаемым положением. Аналогичный код используется для сервоуправления №4, который отслеживает угол наклона контроллера.

// Enable accelerometer only when the buttons are pressed
        // Rotate gripper (only Z button pressed)
        if (c && !z) {
          roll = roll * 57.0 + 90.0; //convert do degrees
          servo5_speed = abs(angle5 - roll)/10 + 1; //speed proportional do the error between actual and desired angle
          if (roll > angle5) {
            angle5 += servo5_speed;
            display_angles = true;
          }
          if (roll < angle5) {
            angle5 -=servo5_speed;
            display_angles = true;
          }
          s5.write(angle5); //update servo position
        }

Захват закрывается всякий раз, когда нажимаются кнопки C и Z. Когда какая-либо из этих кнопок будет не нажата, манипулятор откроет захват.

//Open/close gripper (both buttons pressed)
      if(z && c) {
        s6.write(90); //close gripper
        display_angles = true;
      }
      else {
        s6.write(45); //open gripper
      }

К концу эскиза есть блок кода. Он отобразит на Serial Monitor фактический угол каждого сервомотора. Может быть полезно выбрать начальный угол каждого двигателя.

Шаг 13. Использование

Загрузите скетч файл Arduino.

Теперь, когда все готово, запустите манипулятор и получайте удовольствие!

Nunchuk используется для управления пятью движениями, показанными на рисунках: вращение вправо / влево, движение вверх / вниз, вращение захвата, захват вверх / вниз и захват / открытие. Вы можете комбинировать эти движения для выполнения различных задач.

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

Роботизированные руки с каждым годом находят все большее применение в жизни современного общества, особенно в тех приложениях, в которых требуется скорость, точность и безопасность действий. Их можно не только увидеть в современных кинофильмах, например, в серии фильмов «Железный человек», но их еще в большом количестве и ассортименте производят такие компании как Fanuc, Kuka, Denso, ABB, Yaskawa и т.д. Эти роботизированные руки используются на производственных линиях по сборке автомобилей, горнодобывающей промышленности, химических заводах и т.п.

В этой статье мы рассмотрим создание роботизированной руки (Robotic Arm) с помощью платы Arduino и сервомоторов MG995. Эта роботизированная рука будет иметь 4 степени свободы (без учета захвата) и управляться с помощью потенциометров. Кроме этого, мы также сможем записывать и в дальнейшем воспроизводить все движения руки – именно так во многих случаях и программируются роботизированные руки для работы на реальных производствах.

Необходимые компоненты

1. Плата Arduino Nano (купить на AliExpress).
2. Сервомотор MG-995 (5 шт.) (купить на AliExpress).
3. Потенциометр (5 шт.) (купить на AliExpress).
4. Перфорированная плата.
5. Скелет роботизированной руки и крепления.

Примечание: скелет данной роботизированной руки полностью напечатан на 3D принтере. Если у вас есть доступ к 3D принтеру, то вы можете самостоятельно напечатать все части этой руки на нем по приведенным ниже файлам. Если у вас нет доступа к 3D принтеру, то вы можете изготовить скелет этой руки из акрилового волокна или дерева. В простейшем случае вы можете изготовить скелет этой руки из обычных листов картона, как сделано в простейшей роботизированной руке на основе платы Arduino.

Вначале мы пытались разработать дизайн этой руки самостоятельно, но потом обнаружили что на сервисе Thingiverse есть достаточно много потрясающих дизайнов подобных роботизированных рук и решили «не изобретать заново велосипед». Мы обнаружили, что роботизированная рука Robotic Arm V2.0 by Ashing будет прекрасно работать с сервомоторами MG995, поэтому она отлично подходит для нашего проекта.

Поэтому перейдите по приведенной ссылке на Thingiverse и скачайте там файлы модели этой руки. Всего рука содержит 14 компонентов для 3D печати, STL файлы для всех из них можно скачать по приведенной ссылке. Мы использовали программное обеспечение Cura 3.2.1 от Ultimaker для обработки STL файлов и 3D принтер TEVO tarantula для печати компонентов руки.

Дизайн всех компонентов этой руки достаточно прост, поэтому его можно напечатать практически на любом 3D принтере. У нас на печать всех компонентов руки ушло примерно 4,5 часа. Инструкция по сборке руки приведена на этой странице, поэтому в нашей статье не будем останавливаться на ней подробно.

Единственный момент, на который нам бы хотелось обратить внимание – возможно вам придется вручную подровнять (подшлифовать) края некоторых компонентов чтобы «втиснуть» туда сервомотор — обычно крепления для сервомоторов проектируются с таким расчетом, чтобы запихивать в них сервомоторы приходилось с некоторым усилием, так они будут лучше держаться. Вам понадобится 20 болтов диаметром 3 мм для сборки компонентов этой роботизированной руки.

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

Работа схемы

Схема роботизированной руки на Arduino представлена на следующем рисунке.

Поскольку сервомоторы MG995 работают от питающего напряжения 5V, то их можно запитать от соответствующего разъема платы Arduino. Управляющие контакты сервомоторов подключены к цифровым контактам платы Arduino, на которых возможно формирование сигналов ШИМ (широтно-импульсной модуляции). Потенциометры подключены к аналоговым контактам платы Arduino для управления сервомоторами.

В любой момент времени у нас будет двигаться только один сервомотор, поэтому потребляемый ток не превысит 150 мА, в связи с чем регулятор питания платы Arduino без проблем выдержит такую нагрузку. Сама плата Arduino в нашем проекте запитывалась через USB кабель от компьютера.

После пайки и сборки компонентов на перфорированной плате у нас получилась конструкция, показанная на следующем рисунке. Дополнительно в конструкцию проекта мы добавили разъем для подключения батарейки если в дальнейшем потребуется питание от нее.

Если вы раньше не сталкивались с сервомоторами рекомендуем прочитать статью про подключение сервомотора к плате Arduino.

Объяснение программы для Arduino

В программе мы должны предусмотреть возможность записи движений пользователя по вращению ручек потенциометров и их воспроизведения когда это потребуется. Поэтому в программе нам необходимо два режима – режим записи (Record mode) и режим воспроизведения (Play mode). Пользователь сможет переключаться между этими двумя режимами с помощью монитора последовательной связи (serial monitor). Полный текст программы приведен в конце данной статьи, здесь же мы рассмотрим его наиболее важные фрагменты.

Как обычно, программу начнем с подключения необходимых заголовочных файлов. Мы будем использовать библиотеку Servo.h для управления сервомоторами. У нас 5 сервомоторов в проекте, поэтому каждому из них необходимо дать уникальное имя. Также мы инициализируем переменные, которые затем будем использовать в программе. Мы сделали все переменные глобальными – но вы самостоятельно можете оптимизировать эту часть программы если захотите. Также мы инициализировали массив saved_data, в который мы будем записывать движения руки.

Внутри функции void setup мы инициализируем последовательную связь со скоростью 9600 бод/с. Также мы укажем плате Arduino к каким ее контактам подключены сервомоторы – в нашем случае это контакты 3,5,6,9 и 10. Также в функции setup поскольку она запускается при подаче питания на плату Arduino мы зададим начальное положение руки с помощью поворотов осей сервомоторов на заданные углы. Также в функции setup мы выведем в окно монитора последовательной связи сообщение с просьбой нажать клавишу R или P чтобы потом мы могли выполнить заданное действие (запись или воспроизведение).

Также мы запрограммируем функцию Read_POT, которая будет считывать аналоговые значения с выходов всех потенциометров и преобразовывать их в значения для позиций сервомоторов. Как мы знаем, значения на выходе АЦП (аналогово-цифровой преобразователь) платы Arduino могут изменяться в диапазоне 0-1023, а позиция осей сервомоторов может изменяться только в диапазоне 0-180. Поскольку сервомоторы у нас бюджетные и потому не очень точные, то в целях безопасности целесообразно сузить диапазон их используемых углов поворота с 0-180 до 0-170. Поэтому мы будем использовать функцию map чтобы конвертировать диапазон 0-1023 в диапазон 10-170 для всех наших 5 сервомоторов.

Код для режима записи

В режиме записи мы должны записывать все движения руки – то есть позицию и номер каждого сервомотора. Эти данные мы будем записывать в массив saved_data.

При работе с сервомоторами существует такая проблема как их дрожание/колебание (jitter) во время их работы. Для решения этой проблемы существует множество способов. Но первоначально вы должны определить из-за чего именно возникает эта проблема – из-за схемы управления сервомотором или из-за неточности значения позиции, которая записывается в сервомотор. В нашем случае мы использовали монитор последовательной связи и обнаружили что значение позиции сервомотора не является постоянным и иногда колеблется/изменяется случайным образом.

Таким образом, мы запрограммировали плату Arduino таким образом, чтобы считывать значения с выходов потенциометров дважды и затем сравнивать их между собой. Значение признается истинным (правильным) только тогда когда оба значения совпадают, иначе значение игнорируется. И этот прием решил проблему колебания/джиттера сервомоторов в нашем случае. Также убедитесь в том, что потенциометры надежно (крепко) подсоединены к аналоговым контактам платы Arduino (в нашем случае они припаяны). Любое не очень надежное соединение может приводить к джиттеру. Переменная P_x_pos используется для сохранения старых значений потенциометров, а новые значения потенциометров считываются и преобразуются в заданный диапазон с помощью ранее описанной функции Read_POT.

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

Число разделитель в массиве для нулевого сервомотора (Servo_0)  у нас будет 0, для Servo_1 – 1000 и т.д. для Servo_3 – 3000, а для захвата (Gripper) – 4000. Фрагмент кода, в котором в массив записываются наши данные с добавлением разделителя, выглядит следующим образом:

Код для режима воспроизведения

Когда у нас все движения сервомоторов уже записаны в массиве saved_data мы можем переключиться в режим воспроизведения этих действий при помощи ввода символа ‘P” в окне монитора последовательной связи. В режиме воспроизведения у нас есть доступ к каждому элементу этого массива и, зная известные нам разделители, мы можем разделить значения этого массива чтобы извлечь из них номер мотора и его позицию.

Мы будем использовать цикл for для навигации по элементам массива. Затем в две переменные с именами action_servo и action_pos мы будем записывать номер сервомотора и его позицию соответственно. Чтобы определить номер сервомотора мы будем делить значение из массива на 1000, а позиция сервомотора будет извлекаться из трех последних цифр этого числа (значения из массива).

К примеру, если в массиве сохранено значение 3125, то это будет означать, что 3-й сервомотор необходимо повернуть в позицию 125.

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

Основная функция loop

Внутри основной функции программы loop нам необходимо всего лишь проверять что пользователь ввел в окне монитора последовательной связи и, соответственно, запускать на выполнение режим записи или режим воспроизведения. Если пользователь нажал ‘R’ – активируется режим записи, а если ‘P’ – режим воспроизведения.

Тестирование работы роботизированной руки

Сделайте все соединения, которые показаны на выше приведенной схеме, и загрузите программу в плату Arduino. Подайте питание на плату Arduino Nano через порт USB с вашего компьютера и откройте окно монитора последовательной связи – в нем вы увидите приветственное сообщение.

Теперь введите R в окне монитора последовательной связи и нажмите ввод. Внизу монитора последовательной связи должна быть установлена опция Newline. Пример работы программы в этом режиме показан на следующем рисунке:

Информация, показанная на этом рисунке, может быть использована для отладки. Цифры, начинающиеся с 69, это текущая позиция сервомоторов с 0-го до 5-го. Значения индекса – это размер массива. Помните, что максимальный размер массива ограничен 700 числами, поэтому старайтесь не превышать этот размер. После того как вы завершите запись нажмите P и ввод в окне монитора последовательной связи и программа переключится в режим воспроизведения и на экране тогда появится примерно следующая картина:

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

Исходный код программы (скетча)

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

Видео, демонстрирующее работу роботизированной руки

Загрузка…

8 067 просмотров

Шасси с манипулятором и управлением по Bluetooth

В этом пошаговом руководстве рассматривается создание дистанционно управляемого робота на основе двухколёсного шасси и манипулятора. Управляется робот по Bluetooth из мобильного Android-приложения.

Arduino UNO и Genuino UNO × 1
Манипулятор с двумя степенями свободы (захват и основание) × 1
Модуль Bluetooth HC-05 × 1
Шилд драйвера моторов на L293D × 1
Шасси 2WD Smart Robot × 1
NiMH аккумулятор 5В 1500мАч (4 АА аккумулятора последовательно) × 1
Комплект соединительных проводов Папа-Папа для макетной платы × 1

Программы и онлайн-сервисы:

Arduino IDE
App Inventor

Вступление

Из этого пошагового руководства вы узнаете, как создать робота с манипулятором и управлением с Android устройства. Приложение создано с помощью App Inventor. Каждая часть проекта будет рассмотрена отдельно:

1. Как управлять сервоприводами
2. Как управлять мотор-редукторами с коллекторными моторами
3. Как использовать Bluetooth модуль HC-05
4. Как управлять роботом с помощью мобильного телефона

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

Управление

Прежде всего, мы должны знать, как робот будет принимать команды. Для передачи данных используется модуль Bluetooth, который подключается к Android устройству (смартфон, планшет), на котором запущено приложение, созданное в App Inventor. На данный момент нам нужно знать, что мы отправляем команды роботу с нашего мобильного приложения по Bluetooth.

Электроника

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

Собранное шасси, используемое мной, выглядит так:

MotorShield для Arduino позволяет управлять мотор-редукторами, установленными на нашем шасси. Если вы подключите такие мотор-редукторы на прямую к плате Arduino, она выйдет из строя. Это связано с тем, что каждому такому мотору требуется не менее 80 мА, а максимальный ток, который может обеспечить каждый порт или вывод Arduino, составляет 30мА. Суммарный ток всего микроконтроллера, не более 150ма. Поэтому не пытайтесь подключить такие или более мощные моторы напрямую к плате Arduino.

Шилд моторов помогает вам управлять направлением движения шасси, например, двигаться вперед, назад, повернуть или остановиться.

Драйвер моторов L293D позволяет управлять двумя коллекторными моторами. На плате таких драйвера два, т.е. суммарно можно управлять до 4 коллекторными моторами. Драйвер позволяет задавать направление вращения для каждого мотора, а также использовать ШИМ для регулировки скорости вращения.

Шилд разработан для установки в некоторые платы Arduino, например, Uno или Mega. Для удобства подключения, и чтобы иметь возможность использовать контакты Arduino, к выводам сверху припаять PLS штекер.

Сам шилд моторов устанавливается на Arduino следующим образом:

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

Модуль Bluetooth HC-05

Это модуль UART-моста, который позволяет без проводов управлять роботом. 

Вместо модуля HC-05 можно использовать другой подобный. Модули HC-05 бывают немного разные, но это не существенно. У используемого модуля выведено 6 контактов, для работы используется только средние четыре контакта (RX, TX, GND, +5V):

Подключение:

5В на модуле Bluetooth к 5v на Arduino
GND на модуле Bluetooth к GND на Arduino
Rx на модуле Bluetooth к Tx на Arduino
Tx на модуле Bluetooth к Rx на Arduino

Обратите внимание, что Rx подключается к Tx, а не Tx к Tx и Rx к Rx. Вывод Rx (receiver, приёмник) служит только для приёма данных, а Tx (transmitter, передатчик) только для передачи. По аналогии, это как динамик и микрофон — динамик излучает звуки, а микрофон их регистрирует и «соединять» микрофон с микрофоном или динамик с динамиком не имеет смысла.

Сервоприводы манипулятора

У сервоприводов 3 провода: красный (плюс питания), коричневый (земля), желтый или оранжевый (управляющий).

Не запитывайте сервоприводы от платы Arduino или она выйдет из строя, т.к. не рассчитана на такие большие токи — каждый сервопривод может потреблять два и более ампера, на что схема питания Arduino совершенно не рассчитана.

Для питания используйте внешний источник питания. В данном случае используется аккумулятор, а для тестов можно подключить к внешнему блоку питания или сразу. Земля (GND) источника питания (аккумулятора или внешнего блока питания) и Arduino должны быть соединены.

Красный провод к плюсу питания внешнего источника питания. Коричневый провод и к земле внешнего источника питания и к земле Arduino. Оранжевые провода к любым цифровым выводам на Arduino.

Я подключил сервоприводы к выводам d9 и d10:

gripperServo.attach(9);

elbowServo.attach(10);

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

Мотор-редукторы

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

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

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

Моторы можно подключить к выводам «Мотор 1» и «Мотор 2» или «Мотор 3» и «Мотор 4». В данном случае моторы подключены к выводам «Мотор 1» и «Мотор 2», если подключите к 3 и 4, не забудьте внести изменения в коде.

Аккумулятор подключается к специальному разъёму на плате шилда. Что бы было легко отключать питание, аккумулятор можно подключать через разъём или на проводе «+питания» припаять выключатель.

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

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

Bluetooth

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

Модуль подключается к аппаратному последовательному интерфейсу Arduino, по которому загружается скетч. Чтобы модуль не мешал программированию, перед загрузкой скетча отключите ему питание (провод +5В), а после загрузки скетча снова подключите.

Это демонстрационный проект, возможно вам захочется на его основе сделать что-то своё и при программировании нужно будет часто загружать скетч. В таком случае постоянно отключать питание будет не очень удобно и модуль блютуза можно подключить к другим выводам Arduino – загрузка скетча будет производиться по аппаратному UART, а для блютуза использовать программную реализацию. Программный UART реализован в библиотеке SoftwareSerial.

Программа

Приложение для Android можно скачать по следующей

ссылке

.
Скетч:

#include<AFMotor.h>
#include<Servo.h>
AF_DCMotor motorR(1);
AF_DCMotor motorL(2);
Servo elbowServo;
Servo gripperServo;
int command;

void setup()
{
  gripperServo.attach(9);
  elbowServo.attach(10);
  Serial.begin(9600);
  motorR.setSpeed(255);
  motorL.setSpeed(255);
}

void loop()
{
  command = Serial.read();

  /*     ARM Code    */

  if (command >= 1 && command <= 180) //elbow servo move according to the thumb position on the mob app between 0 -- 180  .
  {
    elbowServo.write(command);
  }
  else if (command == 205)    //Gripper Move To Angle 0
  {
    gripperServo.write(0);
  }
  else if (command == 206)    //Gripper movw to angle 90
  {
    gripperServo.write(90);
  }
  else if (command == 207)    //gripper move to angle 180
  {
    gripperServo.write(180);
  }

  /*  CAR CODE  */

  else if (command == 200)
  {
    motorR.run(FORWARD);
    motorL.run(FORWARD);
  }
  else if (command == 201)
  {
    motorR.run(FORWARD);
    motorL.run(BACKWARD);
  }
  else if (command == 202)
  {
    motorR.run(RELEASE);
    motorL.run(RELEASE);
  }
  else if (command == 203)
  {
    motorR.run(BACKWARD);
    motorL.run(FORWARD);
  }
  else if (command == 204)
  {
    motorR.run(BACKWARD);
    motorL.run(BACKWARD);
  }
  else if (command == 0)
  {
    motorR.run(RELEASE);
    motorL.run(RELEASE);
  }
}

Для работы скетча нужны две библиотеки, AFMotor и Servo. Если они не подключены, подключить их можно в диспетчере библиотек. AFMotor в менеджере библиотек называется «Adafruit Motor Shield library», для используемого шилда подходит первая версия библиотеки.

Автор: Ahmed Ibrahim Ahmed

Перевод

: RobotoTehnika.ru

Впечатлённый возможностями интеллектуального робота-помощника Baxter от Rethink Robotics, робототехник из США, распечатал на 3D-принтере запчасти для простого манипулятора на базе сервомашинок (с выведенным датчиком положения) и написал простой скетч для контроллера Arduino, который считывает положение сервомашинок и сохраняет его в EEPROM. Сохранённые значения, затем можно воспроизводить, что позволяет «обучить» манипулятор любым движениям.
Как показывает пример робота Baxter — это может быть очень полезно для новых промышленных роботов. Т.к. позволяет «перепрограммировать» робота любому работнику.

Видео работы:

Скетч на гитхабе:

#include <Servo.h>
#include <EEPROM.h>

// Servo declarations
Servo one;
Servo two;
Servo three;
Servo four;
Servo five;
// Calibration values
int minDegOne, minDegTwo, minDegThree, minDegFour, minDegFive;
int maxDegOne, maxDegTwo, maxDegThree, maxDegFour, maxDegFive;
int minFeedOne, minFeedTwo, minFeedThree, minFeedFour, minFeedFive;
int maxFeedOne, maxFeedTwo, maxFeedThree, maxFeedFour, maxFeedFive;
int posOne, posTwo, posThree, posFour, posFive;
int posOne1, posTwo1, posThree1, posFour1, posFive1;
int addr = 0;
boolean recorded = false;

void setup()
{
  Serial.begin(9600);
  one.attach(8);
  two.attach(9);
  three.attach(10);
  four.attach(11);
  five.attach(12);
  pinMode(13, OUTPUT);  // LED
  pinMode(6, INPUT);    // Replay Button
  pinMode(7, INPUT);    // Train Button
  delay(100);
  // One center to left
  for (int i = 90; i > 29; i--)
  {
    one.write(i);
    delay(10);
  }
  minDegOne = 30;
  minFeedOne = analogRead(1);
  delay(500);
  // One left to right
  for (int i = 30; i < 151; i++)
  {
    one.write(i);
    delay(10);
  }
  maxDegOne = 150;
  maxFeedOne = analogRead(1);
  delay(500);
  // One right to center
  for (int i = 150; i > 89; i--)
  {
    one.write(i);
    delay(10);
  }
  delay(500);
  // Two up to forward
  for (int i = 90; i > 29; i--)
  {
    two.write(i);
    delay(10);
  }
  minDegTwo = 30;
  minFeedTwo = analogRead(2);
  delay(500);
  // Two forward to backward
  for (int i = 25; i < 151; i++)
  {
    two.write(i);
    delay(10);
  }
  maxDegTwo = 150;
  maxFeedTwo = analogRead(2);
  delay(500);
  // Two backward to up
  for (int i = 150; i > 89; i--)
  {
    two.write(i);
    delay(10);
  }
  delay(500);
  // Three up to forward
  for (int i = 90; i > 30; i--)
  {
    three.write(i);
    delay(10);
  }
  minDegThree = 30;
  minFeedThree = analogRead(3);
  delay(500);
  // Three forward to backward
  for (int i = 30; i < 151; i++)
  {
    three.write(i);
    delay(10);
  }
  maxDegThree = 150;
  maxFeedThree = analogRead(3);
  delay(500);
  // Three backward to up
  for (int i = 150; i > 89; i--)
  {
    three.write(i);
    delay(10);
  }
  delay(500);
  // Four up to forward
  for (int i = 90; i > 29; i--)
  {
    four.write(i);
    delay(10);
  }
  minDegFour = 30;
  minFeedFour = analogRead(4);
  delay(500);
  // Four forward to backward
  for (int i = 30; i < 151; i++)
  {
    four.write(i);
    delay(10);
  }
  maxDegFour = 150;
  maxFeedFour = analogRead(4);
  delay(500);
  // Four backward to up
  for (int i = 150; i > 89; i--)
  {
    four.write(i);
    delay(10);
  }
  delay(500);
  // Five up to forward
  for (int i = 90; i > 19; i--)
  {
    five.write(i);
    delay(10);
  }
  minDegFive = 20;
  minFeedFive = analogRead(5);
  delay(500);
  // Five forward to backward
  for (int i = 20; i < 181; i++)
  {
    five.write(i);
    delay(10);
  }
  maxDegFive = 180;
  maxFeedFive = analogRead(5);
  delay(500);
  // Five backward to up
  for (int i = 180; i > 89; i--)
  {
    five.write(i);
    delay(10);
  }
  delay(500);
  // Center all servos
  one.write(90);
  two.write(90);
  three.write(90);
  four.write(90);
  five.write(90);
  delay(1000);
  // Detach to save power and allow human manipulation
  one.detach();
  two.detach();
  three.detach();
  four.detach();
  five.detach();
  /*
  // Display minimums and maximums for analog feedback
  // Uncomment for debugging
  Serial.print("One Min: ");
  Serial.println(minFeedOne);
  Serial.print("One Max: ");
  Serial.println(maxFeedOne);
  Serial.print("Two Min: ");
  Serial.println(minFeedTwo);
  Serial.print("Two Max: ");
  Serial.println(maxFeedTwo);
  Serial.print("Three Min: ");
  Serial.println(minFeedThree);
  Serial.print("Three Max: ");
  Serial.println(maxFeedThree);
  Serial.print("Four Min: ");
  Serial.println(minFeedFour);
  Serial.print("Four Max: ");
  Serial.println(maxFeedFour);
  Serial.print("Five Min: ");
  Serial.println(minFeedFive);
  Serial.print("Five Max: ");
  Serial.println(maxFeedFive);
  Serial.println();
  */
}

void loop()
{
  delay(100);
  if (digitalRead(7))
  {
    recorded = true;
    digitalWrite(13, HIGH);
    delay(1000);
    while (!digitalRead(7))
    {
      delay(50);
      int posOne = map(analogRead(1), minFeedOne, maxFeedOne, minDegOne, maxDegOne);
      EEPROM.write(addr, posOne);
      addr++;
      int posTwo = map(analogRead(2), minFeedTwo, maxFeedTwo, minDegTwo, maxDegTwo);
      EEPROM.write(addr, posTwo);
      addr++;
      int posThree = map(analogRead(3), minFeedThree, maxFeedThree, minDegThree, maxDegThree);
      EEPROM.write(addr, posThree);
      addr++;
      int posFour = map(analogRead(4), minFeedFour, maxFeedFour, minDegFour, maxDegFour);
      EEPROM.write(addr, posFour);
      addr++;
      int posFive = map(analogRead(5), minFeedFive, maxFeedFive, minDegFive, maxDegFive);
      EEPROM.write(addr, posFive);
      addr++;
      if (addr == 512)
      {
        EEPROM.write(addr, 255);
        break;
      }
      delay(50);
    }
    EEPROM.write(addr, 255);
  }
  if (recorded || digitalRead(6))
  {
    digitalWrite(13, LOW);
    // Power up servos
    one.attach(8);
    two.attach(9);
    three.attach(10);
    four.attach(11);
    five.attach(12);
    delay(1000);
    // Center servos
    one.write(90);
    two.write(90);
    three.write(90);
    four.write(90);
    five.write(90);
    delay(1000);
    // Start playback
    addr = 0;
    while (1)
    {
      posOne = EEPROM.read(addr);
      posOne1 = EEPROM.read(addr+5);
      addr++;
      posTwo = EEPROM.read(addr);
      posTwo1 = EEPROM.read(addr+5);
      addr++;
      posThree = EEPROM.read(addr);
      posThree1 = EEPROM.read(addr+5);
      addr++;
      posFour = EEPROM.read(addr);
      posFour1 = EEPROM.read(addr+5);
      addr++;
      posFive = EEPROM.read(addr);
      posFive1 = EEPROM.read(addr+5);
      addr++;

      /*
      // Display positions being written to the servos
      // Uncomment for debugging
      Serial.print("One: ");
      Serial.print(posOne);
      Serial.print("ttt");
      Serial.println(posOne1);
      Serial.print("Two: ");
      Serial.print(posTwo);
      Serial.print("tt");
      Serial.println(posTwo1);
      Serial.print("Three: ");
      Serial.print(posThree);
      Serial.print("tt");
      Serial.println(posThree1);
      Serial.print("Four: ");
      Serial.print(posFour);
      Serial.print("tt");
      Serial.println(posFour1);
      Serial.print("Five: ");
      Serial.print(posFive);
      Serial.print("tt");
      Serial.println(posFive1);
      Serial.println();
      */

      // Check for the end of the recorded commands, if so then break out of the infinite loop
      if ((posOne == 255) || (posOne1 == 255) || (posTwo == 255) || (posTwo1 == 255) || (posThree == 255) || (posThree1 == 255) || (posFour == 255) || (posFour1 == 255) || (posFive == 255) || (posFive1 == 255))
      {
        break;
      }

      // Step from one recording to the next for each servo
      if ((posOne1 - posOne) > 0)
      {
        for (int i = posOne; i < posOne1; i++)
        {
          one.write(i);
          delay(5);
        }
      }
      else if ((posOne1 - posOne) < 0)
      {
        for (int i = posOne; i > posOne1; i--)
        {
          one.write(i);
          delay(5);
        }
      }
      if ((posTwo1 - posTwo) > 0)
      {
        for (int i = posTwo; i < posTwo1; i++)
        {
          two.write(i);
          delay(5);
        }
      }
      else if ((posTwo1 - posTwo) < 0)
      {
        for (int i = posTwo; i > posTwo1; i--)
        {
          two.write(i);
          delay(5);
        }
      }
      if ((posThree1 - posThree) > 0)
      {
        for (int i = posThree; i < posThree1; i++)
        {
          three.write(i);
          delay(5);
        }
      }
      else if ((posThree1 - posThree) < 0)
      {
        for (int i = posThree; i > posThree1; i--)
        {
          three.write(i);
          delay(5);
        }
      }
      if ((posFour1 - posFour) > 0)
      {
        for (int i = posFour; i < posFour1; i++)
        {
          four.write(i);
          delay(5);
        }
      }
      else if ((posFour1 - posFour) < 0)
      {
        for (int i = posFour; i > posFour1; i--)
        {
          four.write(i);
          delay(5);
        }
      }
      if ((posFive1 - posFive) > 0)
      {
        for (int i = posFive; i < posFive1; i++)
        {
          five.write(i);
          delay(5);
        }
      }
      else if ((posFive1 - posFive) < 0)
      {
        for (int i = posFive; i > posFive1; i--)
        {
          five.write(i);
          delay(5);
        }
      }
    }
    recorded = false;
    addr = 0;
    delay(1000);
    // Center all servos
    one.write(90);
    two.write(90);
    three.write(90);
    four.write(90);
    five.write(90);
    delay(500);
    // Detach them to save power and allow human manipulation
    one.detach();
    two.detach();
    three.detach();
    four.detach();
    five.detach();
  }
}

Ссылки
Learning Robotic Arm

По теме
Программирование Arduino — библиотека Servo
Самодельный манипулятор под управлением Arduino
InMoov — робо-рука под управлением Arduino
Клешня из ПКЛ
Хаки сервомашинок. Переделка в серву постоянного вращения