Использование Arduino Uno для позиционирования XYZ роботизированной руки с 6 степенями свободы: 4 шага

2024 Автор: John Day | [email protected]. Последнее изменение: 2024-01-30 11:53

Этот проект посвящен реализации короткого и относительно простого скетча Arduino для обеспечения обратного кинематического позиционирования XYZ. Я построил роботизированную руку с 6 сервоприводами, но когда дело дошло до поиска программного обеспечения для его запуска, там не было ничего особенного, за исключением специальных программ, работающих на настраиваемых сервозащитных экранах, таких как SSC-32 (U) или других программах и приложениях, которые были сложно установить и общаться с рукой. Затем я нашел превосходнейшую «Инверсную кинематику манипулятора робота на Arduino» Олега Мазурова, в которой он реализовал инверсную кинематику в простом скетче Arduino.

Я сделал две модификации, чтобы адаптировать его код:

1. Я использовал библиотеку VarSpeedServo вместо его пользовательской библиотеки серво щита, потому что тогда я мог бы контролировать скорость серво, и мне не пришлось бы использовать серво щит, который он использовал. Для тех, кто рассматривает возможность запуска кода, представленного здесь, я рекомендую вам использовать эту библиотеку VarSpeedServo, а не библиотеку servo.h, чтобы вы могли замедлить движение своей роботизированной руки во время разработки или вы можете обнаружить, что рука неожиданно ткнет вас в лицо или того хуже, потому что он будет двигаться на полной скорости сервопривода.

2. Я использую простой датчик / серво щиток для подключения сервоприводов к Arduino Uno, но он не требует специальной библиотеки сервоприводов, поскольку использует только контакты Arduino. Это стоит всего несколько долларов, но это не обязательно. Это обеспечивает хорошее чистое подключение сервоприводов к Arduino. И я никогда не вернусь к подключению сервоприводов к Arduino Uno. Если вы используете этот датчик / сервощит, вам нужно сделать одну небольшую модификацию, которую я опишу ниже.

Код отлично работает и позволяет вам управлять рукой, используя единственную функцию, в которой вы передаете параметры x, y, x и скорость. Например:

set_arm (0, 240, 100, 0, 20); // параметры (x, y, z, угол захвата, скорость сервопривода)

задержка (3000); // требуется задержка, чтобы дать руке время переместиться в это место

Не может быть проще. Я включу эскиз ниже.

Видео Олега здесь: Управление роботизированной рукой с помощью Arduino и USB-мыши

Оригинальная программа Олега, описания и ресурсы: обратная кинематика Олега для Arduino Uno

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

Шаг 1: Модификация оборудования

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

2. Если вы используете экран датчика, который я использую, вам нужно сделать с ним одну вещь: согнуть штырь, соединяющий 5 В от экрана с Arduino Uno, в сторону, чтобы он не подключался к плате Uno. Вы хотите использовать внешнее напряжение на щите для питания только ваших сервоприводов, а не Arduino Uno, иначе это может разрушить Uno, я знаю, поскольку я сжег две платы Uno, когда мое внешнее напряжение было 6 вольт, а не 5. Это позволяет вам использовать более 5 В для питания ваших сервоприводов, но если ваше внешнее напряжение превышает 5 вольт, не подключайте какие-либо датчики на 5 вольт к щиту, иначе они будут поджариваться.

Шаг 2: Загрузите библиотеку VarSpeedServo

Вам необходимо использовать эту библиотеку, которая заменяет стандартную библиотеку сервоприводов Arduino, поскольку она позволяет передавать скорость сервопривода в оператор записи сервопривода. Библиотека находится здесь:

Библиотека VarSpeedServo

Вы можете просто использовать кнопку zip, загрузить zip-файл и затем установить его с помощью Arduino IDE. После установки команда в вашей программе будет выглядеть так: servo.write (100, 20);

Первый параметр - это угол, а второй - скорость сервопривода от 0 до 255 (полная скорость).

Шаг 3: запустите этот эскиз

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

1. BASE_HGT, HUMERUS, ULNA, GRIPPER - длина в миллиметрах.

2. Введите номера штырей сервопривода.

3. Введите min и max сервопривода в операторы присоединения.

4. Затем попробуйте простую команду set_arm (), а затем функции zero_x (), line () и circle () для тестирования. При первом запуске этих функций убедитесь, что скорость вашего сервопривода низкая, чтобы не повредить руку и руку.

Удачи.

#include VarSpeedServo.h

/ * Сервоуправление для руки AL5D * /

/ * Размеры рычага (мм) * /

#define BASE_HGT 90 // базовая высота

#define HUMERUS 100 // "кость" от плеча до локтя

#define ULNA 135 // "кость" от локтя до запястья

#define GRIPPER 200 // длина захвата (включая сверхмощный механизм поворота запястья)"

#define ftl (x) ((x)> = 0? (long) ((x) +0.5):(long) ((x) -0.5)) // преобразование float в long

/ * Имена / номера сервоприводов *

* Базовый сервопривод HS-485HB * /

#define BAS_SERVO 4

/ * Плечевой сервопривод HS-5745-MG * /

#define SHL_SERVO 5

/ * Сервопривод локтя HS-5745-MG * /

#define ELB_SERVO 6

/ * Сервопривод запястья HS-645MG * /

#define WRI_SERVO 7

/ * Сервопривод вращения запястья HS-485HB * /

#define WRO_SERVO 8

/ * Сервопривод захвата HS-422 * /

#define GRI_SERVO 9

/ * предварительные расчеты * /

float hum_sq = HUMERUS * HUMERUS;

float uln_sq = ULNA * ULNA;

int servoSPeed = 10;

// ServoShield servos; // Объект ServoShield

VarSpeedServo servo1, servo2, servo3, servo4, servo5, servo6;

int loopCounter = 0;

int pulseWidth = 6,6;

int microsecondsToDegrees;

установка void ()

{

servo1.attach (BAS_SERVO, 544, 2400);

servo2.attach (ШЛ_СЕРВО, 544, 2400);

servo3.attach (ELB_SERVO, 544, 2400);

servo4.attach (WRI_SERVO, 544, 2400);

servo5.attach (WRO_SERVO, 544, 2400);

servo6.attach (GRI_SERVO, 544, 2400);

задержка (5500);

//servos.start (); // Запуск серво щита

servo_park ();

задержка (4000);

Serial.begin (9600);

Serial.println («Старт»);

}

пустой цикл ()

{

loopCounter + = 1;

// set_arm (-300, 0, 100, 0, 10); //

// задержка (7000);

// zero_x ();

//линия();

//круг();

задержка (4000);

if (loopCounter> 1) {

servo_park ();

// set_arm (0, 0, 0, 0, 10); // парк

задержка (5000);

выход (0); } // приостановить программу - нажмите сброс, чтобы продолжить

// выход (0);

}

/ * процедура позиционирования руки с использованием обратной кинематики * /

/ * z - высота, y - расстояние от центра основания, x - из стороны в сторону. y, z могут быть только положительными * /

// пусто set_arm (uint16_t x, uint16_t y, uint16_t z, uint16_t grip_angle)

void set_arm (float x, float y, float z, float grip_angle_d, int servoSpeed)

{

float grip_angle_r = радианы (grip_angle_d); // угол захвата в радианах для использования в расчетах

/ * Базовый угол и радиальное расстояние от координат x, y * /

поплавок bas_angle_r = atan2 (x, y);

float rdist = sqrt ((х * х) + (у * у));

/ * rdist - координата y для руки * /

y = rdist;

/ * Смещения захвата рассчитываются на основе угла захвата * /

float grip_off_z = (sin (grip_angle_r)) * GRIPPER;

float grip_off_y = (cos (grip_angle_r)) * GRIPPER;

/ * Положение запястья * /

float wrist_z = (z - grip_off_z) - BASE_HGT;

float wrist_y = y - grip_off_y;

/ * Расстояние от плеча до запястья (AKA sw) * /

float s_w = (wrist_z * wrist_z) + (wrist_y * wrist_y);

поплавок s_w_sqrt = sqrt (s_w);

/ * угол s_w относительно земли * /

float a1 = atan2 (wrist_z, wrist_y);

/ * угол s_w к плечевой кости * /

float a2 = acos (((hum_sq - uln_sq) + s_w) / (2 * HUMERUS * s_w_sqrt));

/ * угол плеча * /

поплавок shl_angle_r = a1 + a2;

float shl_angle_d = градусы (shl_angle_r);

/ * угол локтя * /

float elb_angle_r = acos ((hum_sq + uln_sq - s_w) / (2 * HUMERUS * ULNA));

float elb_angle_d = градусы (elb_angle_r);

float elb_angle_dn = - (180.0 - elb_angle_d);

/ * угол запястья * /

float wri_angle_d = (grip_angle_d - elb_angle_dn) - shl_angle_d;

/ * Сервоимпульсы * /

float bas_servopulse = 1500.0 - ((градусы (bas_angle_r)) * ширина импульса);

float shl_servopulse = 1500.0 + ((shl_angle_d - 90.0) * pulseWidth);

float elb_servopulse = 1500.0 - ((elb_angle_d - 90.0) * ширина импульса);

// число с плавающей запятой wri_servopulse = 1500 + (wri_angle_d * pulseWidth);

// число с плавающей запятой wri_servopulse = 1500 + (wri_angle_d * pulseWidth);

float wri_servopulse = 1500 - (wri_angle_d * pulseWidth); // обновлено в 2018/2/11 от jimrd - я изменил плюс на минус - не уверен, как этот код работал у кого-либо раньше. Возможно, сервопривод локтя был установлен с углом 0 градусов вниз, а не вверх.

/ * Устанавливаем сервоприводы * /

//servos.setposition(BAS_SERVO, ftl (bas_servopulse));

microsecondsToDegrees = map (ftl (bas_servopulse), 544, 2400, 0, 180);

servo1.write (microsecondsToDegrees, servoSpeed); // используйте эту функцию, чтобы вы могли установить скорость сервопривода //

//servos.setposition(SHL_SERVO, ftl (shl_servopulse));

microsecondsToDegrees = map (ftl (shl_servopulse), 544, 2400, 0, 180);

servo2.write (microsecondsToDegrees, servoSpeed);

//servos.setposition(ELB_SERVO, ftl (elb_servopulse));

microsecondsToDegrees = map (ftl (elb_servopulse), 544, 2400, 0, 180);

servo3.write (microsecondsToDegrees, servoSpeed);

//servos.setposition(WRI_SERVO, ftl (wri_servopulse));

microsecondsToDegrees = map (ftl (wri_servopulse), 544, 2400, 0, 180);

servo4.write (microsecondsToDegrees, servoSpeed);

}

/ * перемещаем сервоприводы в положение парковки * /

void servo_park ()

{

//servos.setposition(BAS_SERVO, 1500);

servo1.write (90, 10);

//servos.setposition(SHL_SERVO, 2100);

servo2.write (90, 10);

//servos.setposition(ELB_SERVO, 2100);

servo3.write (90, 10);

//servos.setposition(WRI_SERVO, 1800);

servo4.write (90, 10);

//servos.setposition(WRO_SERVO, 600);

servo5.write (90, 10);

//servos.setposition(GRI_SERVO, 900);

servo6.write (80, 10);

возвращение;

}

void zero_x ()

{

for (двойная ось y = 250,0; ось y <400,0; ось y + = 1) {

Serial.print ("yaxis =:"); Serial.println (yaxis);

set_arm (0, ось оси, 200.0, 0, 10);

задержка (10);

}

for (двойная ось y = 400,0; ось y> 250,0; ось y - = 1) {

set_arm (0, ось оси, 200.0, 0, 10);

задержка (10);

}

}

/ * перемещает руку по прямой * /

пустая строка ()

{

for (double xaxis = -100.0; xaxis <100.0; xaxis + = 0.5) {

set_arm (xaxis, 250, 120, 0, 10);

задержка (10);

}

for (float xaxis = 100.0; xaxis> -100.0; xaxis - = 0.5) {

set_arm (xaxis, 250, 120, 0, 10);

задержка (10);

}

}

пустой круг ()

{

#define РАДИУС 50.0

// плавающий угол = 0;

поплавок zaxis, yaxis;

для (угол поплавка = 0,0; угол <360,0; угол + = 1,0) {

yaxis = РАДИУС * sin (радианы (угол)) + 300;

zaxis = РАДИУС * cos (радианы (угол)) + 200;

set_arm (0, ось y, ось z, 0, 50);

задержка (10);

}

}

Шаг 4. Факты, проблемы и т. Д.…

1. Когда я запускаю подпрограмму circle (), мой робот движется больше по эллиптической форме, чем по кругу. Я думаю, это потому, что мои сервоприводы не откалиброваны. Я тестировал один из них, и 1500 микросекунд - это не то же самое, что 90 градусов. Будем работать над этим, чтобы попытаться найти решение. Не верьте, что что-то не так с алгоритмом, а скорее с моими настройками. Обновление 2018/2/11 - только что обнаружил, что это связано с ошибкой в исходном коде. Я не понимаю, как работала его программа. Исправленный код, использующий это: float wri_servopulse = 1500 - (wri_angle_d * pulseWidth); (добавлялся исходный код)

2. Где я могу найти дополнительную информацию о том, как работает функция set_arm (): на сайте Олега Мазурова все объясняется или есть ссылки для получения дополнительной информации:

3. Есть ли проверка граничных условий? Нет. Когда моей руке робота передается недопустимая координата xyz, он делает это забавное изгибающееся движение, как кошка растягивается. Я считаю, что Олег кое-что проверяет в своей последней программе, которая использует USB для программирования движений рук. Посмотрите его видео и дайте ссылку на его последний код.

4. Код необходимо очистить, и можно избавиться от микросекундного кода.