Използване на Arduino Uno за XYZ позициониране на 6 DOF роботизирана ръка: 4 стъпки

2024 Автор: John Day | [email protected]. Последно модифициран: 2024-01-30 07: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. Ако използвате сензорния щит, който използвам, трябва да направите едно нещо за него: огънете щифта, който свързва 5v от щита към Arduino Uno, така че да не се свързва с платката Uno. Искате да използвате външното напрежение на щита, за да захранвате само вашите сервомотори, а не Arduino Uno или може да унищожи Uno, знам, че изгорих две платки Uno, когато външното ми напрежение беше 6 волта, а не 5. Това ви позволява да използвате по -високо от 5v за захранване на вашите сервомотори, но ако външното ви напрежение е по -високо от 5 волта, тогава не свързвайте никакви 5 -волтови сензори към щита или те ще бъдат пържени.

Стъпка 2: Изтеглете библиотеката VarSpeedServo

Трябва да използвате тази библиотека, която замества стандартната arduino servo библиотека, защото ви позволява да предавате скорост на серво в оператора за запис на серво. Библиотеката се намира тук:

Библиотека VarSpeedServo

Можете просто да използвате бутона zip, да изтеглите zip файла и след това да го инсталирате с Arduino IDE. След като инсталирате командата във вашата програма, ще изглежда така: servo.write (100, 20);

Първият параметър е ъгълът, а вторият е скоростта на серво от 0 до 255 (пълна скорост).

Стъпка 3: Изпълнете тази скица

Ето и състезателната програма. Трябва да промените няколко параметъра за размерите на вашата роботизирана ръка:

1. BASE_HGT, HUMERUS, ULNA, GRIPPER дължини в милиметри.

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

3. Въведете servo min и max в прикрепените изявления.

4. След това опитайте проста команда set_arm () и след това функциите zero_x (), line () и circle () за тестване. Уверете се, че скоростта на вашето серво е ниска при първото стартиране на тези функции, за да предотвратите повреда на ръката и собствената си ръка.

Късмет.

#include VarSpeedServo.h

/ * Серво управление за рамото AL5D */

/ * Размери на рамото (мм) */

#define BASE_HGT 90 // базова височина

#дефинирайте HUMERUS 100 // "кост" от рамо до лакът

#define ULNA 135 // "кост" от лакът до китката

#define GRIPPER 200 // грайфер (вкл. механизъм за въртене на китката с тежко натоварване) дължина"

#define ftl (x) ((x)> = 0? (long) ((x) +0.5):(long) ((x) -0.5)) // преобразуване с плаващо към дълго

/ * Серво имена/номера *

* Базово серво HS-485HB */

#дефинирайте BAS_SERVO 4

/ * Серво за рамо HS-5745-MG */

#дефинирайте SHL_SERVO 5

/ * Лакътно серво HS-5745-MG */

#дефинирайте ELB_SERVO 6

/ * Серво за китката HS-645MG */

#дефинирайте WRI_SERVO 7

/ * Серво за въртене на китката HS-485HB */

#дефинирайте WRO_SERVO 8

/ * Сервозахващач HS-422 */

#дефинирайте GRI_SERVO 9

/ * предварителни изчисления */

float hum_sq = HUMERUS*HUMERUS;

поплавък uln_sq = ULNA*ULNA;

int servoSPeed = 10;

// ServoShield серво; // ServoShield обект

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

int loopCounter = 0;

int pulseWidth = 6,6;

int микросекундиToDegrees;

void setup ()

{

servo1.attach (BAS_SERVO, 544, 2400);

servo2.attach (SHL_SERVO, 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 ("Старт");

}

void loop ()

{

loopCounter += 1;

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

// забавяне (7000);

// нула_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 координати */

float bas_angle_r = atan2 (x, y);

float rdist = sqrt ((x * x) + (y * y));

/ * 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);

float 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)) * pulseWidth);

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

float elb_servopulse = 1500.0 - ((elb_angle_d - 90.0) * pulseWidth);

// плаващ 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 = карта (ftl (bas_servopulse), 544, 2400, 0, 180);

servo1.write (microsecondsToDegrees, servoSpeed); // използвайте тази функция, за да можете да зададете скорост на серво //

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

microsecondsToDegrees = карта (ftl (shl_servopulse), 544, 2400, 0, 180);

servo2.write (microsecondsToDegrees, servoSpeed);

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

microsecondsToDegrees = карта (ftl (elb_servopulse), 544, 2400, 0, 180);

servo3.write (microsecondsToDegrees, servoSpeed);

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

microsecondsToDegrees = карта (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 ()

{

за (двойна ос = 250,0; ос <400,0; ос += 1) {

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

set_arm (0, yaxis, 200.0, 0, 10);

забавяне (10);

}

за (двоен yaxis = 400.0; yaxis> 250.0; yaxis -= 1) {

set_arm (0, yaxis, 200.0, 0, 10);

забавяне (10);

}

}

/ * движи ръката по права линия */

празна линия ()

{

за (двойна ос = -100.0; ос <100.0; ос += 0.5) {

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

забавяне (10);

}

for (поплавка xaxis = 100.0; xaxis> -100.0; xaxis -= 0.5) {

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

забавяне (10);

}

}

празен кръг ()

{

#дефинирайте RADIUS 50.0

// поплавъчен ъгъл = 0;

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

за (ъгъл на плаване = 0,0; ъгъл <360,0; ъгъл += 1,0) {

yaxis = RADIUS * sin (радиани (ъгъл)) + 300;

zaxis = RADIUS * cos (радиани (ъгъл)) + 200;

set_arm (0, yaxis, zaxis, 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. Кодът трябва да бъде почистен и микросекундният код може да бъде премахнат.