DOF 6 робот гарыг XYZ байрлуулахад Arduino Uno -ийг ашиглах нь: 4 алхам

2024 Зохиолч: John Day | [email protected]. Хамгийн сүүлд өөрчлөгдсөн: 2024-01-30 11:04

Энэхүү төсөл нь XYZ кинематик урвуу кинематик байрлалыг хангах богино бөгөөд харьцангуй хялбар Arduino ноорогыг хэрэгжүүлэх тухай юм. Би 6 servo робот гар бүтээсэн боловч үүнийг ажиллуулах програм хангамжийг хайж олоход SSC-32 (U) гэх мэт тусгай зориулалтын servo shield дээр ажилладаг өөрчлөн тохируулсан програмууд эсвэл бусад програм, програмуудаас бусад нь тийм ч их байсангүй. суулгах, гартай холбоо тогтооход төвөгтэй. Дараа нь би Олег Мазуровын хамгийн шилдэг "Arduino дээрх робот гарны урвуу кинематик" -ыг олсон бөгөөд тэрээр урвуу кинематикийг энгийн Arduino ноорогоор хэрэгжүүлсэн юм.

Би түүний кодыг өөрчлөхийн тулд хоёр өөрчлөлт хийсэн.

1. Би VarSpeedServo номын санг өөрийн тусгай servo shield номын сангийн оронд ашигласан, учир нь би servo -ийн хурдыг хянах боломжтой байсан бөгөөд түүний ашигладаг servo shield -ийг ашиглах шаардлагагүй болно. Энд өгөгдсөн кодыг ажиллуулахыг хүсч буй хэн бүхэнд servo.h номын сангаас илүү энэхүү VarSpeedServo номын санг ашиглахыг зөвлөж байна, ингэснээр та хөгжүүлэлтийн явцад роботын гарны хөдөлгөөнийг удаашруулж чадна, эс тэгвээс гар нь таныг гэнэт цохих болно. нүүр эсвэл бүр дорддог, учир нь энэ нь бүрэн servo хурдаар хөдлөх болно.

2. Би servo -г Arduino Uno -той холбохын тулд энгийн мэдрэгч/servo shield ашигладаг боловч энэ нь зөвхөн Arduino -ийн тээглүүрийг ашигладаг тул тусгай servo номын сан шаарддаггүй. Энэ нь хэдхэн долларын үнэтэй боловч үүнийг хийх шаардлагагүй юм. Энэ нь servo -г Arduino руу цэвэрхэн холбох боломжийг олгодог. Би одоо хэзээ ч Arduino Uno -ийн хатуу хөтөч рүү буцахгүй. Хэрэв та энэ мэдрэгч/servo бамбайг ашиглавал миний доор тайлбарлах нэг жижиг өөрчлөлт хийх шаардлагатай болно.

Код нь маш сайн ажилладаг бөгөөд x, y, x болон хурдны параметрүүдийг дамжуулдаг ганц функцийг ашиглан гараа ажиллуулах боломжийг олгодог. Жишээлбэл:

set_arm (0, 240, 100, 0, 20); // параметрүүд нь (x, y, z, gripper өнцөг, servo speed)

саатал (3000); // энэ байрлал руу шилжих цагийг олгохын тулд хойшлуулах шаардлагатай

Илүү хялбар байж чадахгүй. Би доорх тоймыг оруулна.

Олегийн видео энд байна: Arduino болон USB хулганы тусламжтайгаар робот гарыг удирдах нь

Олегийн анхны програм, тодорхойлолт, эх сурвалж: Arduino Uno -д зориулсан Олегийн урвуу кинематик

Хичээлийн цаана байгаа бүх математикийг би ойлгодоггүй, гэхдээ гоё зүйл бол та кодыг ашиглах шаардлагагүй юм. Та үүнийг туршиж үзээрэй гэж найдаж байна.

Алхам 1: Тоног төхөөрөмжийн өөрчлөлт

1. Шаардлагатай цорын ганц зүйл бол таны servo хүлээгдэж буй чиглэлд эргэх бөгөөд ингэснээр таны servo -ийн бэхэлгээг биеэрээ эргүүлэх шаардлагатай болно. Суурь, мөр, тохой, бугуйны servo -ийн хүлээгдэж буй servo чиглэлийг харахын тулд энэ хуудас руу очно уу:

2. Хэрэв та миний ашиглаж байгаа мэдрэгчийн бамбайг ашиглаж байгаа бол үүнийг хийхийн тулд нэг зүйлийг хийх хэрэгтэй: 5в -ийг бамбайгаас Arduino Uno руу холбосон зүүг Uno -ийн самбартай холбогдоогүй байх ёстой. Та бамбай дээрх гадаад хүчдэлийг Arduino Uno -г биш зөвхөн өөрийн servo -оо тэжээхийг хүсч байна, эсвэл энэ нь Uno -г сүйтгэж болзошгүй юм. Миний гадаад хүчдэл 5 вольт биш 6 вольт байхад би хоёр Uno самбарыг шатаасан гэдгийг би мэднэ. 5В -оос дээш хүчээр servo -оо тэжээх боловч хэрэв таны гадаад хүчдэл 5 вольтоос дээш байвал 5 вольтын мэдрэгчийг бамбай руу бүү холбоорой, эс тэгвэл тэд шарсан болно.

Алхам 2: VarSpeedServo номын санг татаж авах

Та стандарт arduino servo номын санг орлох энэхүү номын санг ашиглах хэрэгтэй, учир нь энэ нь servo бичих мэдэгдэлд servo хурдыг дамжуулах боломжийг олгодог. Номын сан энд байрладаг:

VarSpeedServo номын сан

Та зүгээр л zip товчлуурыг ашиглаж, зип файлыг татаж аваад Arduino IDE ашиглан суулгаж болно. Таны програмд командыг суулгасны дараа иймэрхүү харагдах болно: servo.write (100, 20);

Эхний параметр нь өнцөг бөгөөд хоёр дахь нь servo -ийн 0 -ээс 255 хүртэлх хурд (бүрэн хурд) юм.

Алхам 3: Энэ ноорогыг ажиллуулна уу

Энд өрсөлдөх хөтөлбөр байна. Та роботын гарны хэмжээг тодорхойлох хэд хэдэн параметрийг өөрчлөх хэрэгтэй.

1. BASE_HGT, HUMERUS, ULNA, GRIPPER уртууд миллиметрээр.

2. Серво зүү дугаараа оруулна уу

3. Хавсралт мэдэгдэлд servo min ба max -ийг оруулна уу.

4. Дараа нь энгийн set_arm () командыг туршиж үзэхийн тулд zero_x (), line () болон тойрог () функцуудыг туршиж үзээрэй. Таны гар болон өөрийн гарыг гэмтээхгүйн тулд эдгээр функцүүдийг ажиллуулахдаа таны servo хурд бага байгаа эсэхийг шалгаарай.

Амжилт хүсье.

#VarSpeedServo.h -ийг оруулна уу

/ * AL5D гарны servo control */

/ * Гарны хэмжээ (мм) */

#BASE_HGT 90 // суурийн өндрийг тодорхойлох

#deumine HUMERUS 100 // мөрнөөс тохой хүртэл "яс"

#тодорхойлох ULNA 135 // тохойноос бугуй хүртэл "яс"

#тодорхойлох GRIPPER 200 // бариул (бугуйны хүнд эргэлтийн механизмыг оруулаад) урт"

#define ftl (x) ((x)> = 0? (урт) ((x) +0.5):(урт) ((x) -0.5)) // урт хөрвүүлэлт рүү хөвөх

/ * Серво нэр/дугаар *

* HS-485HB үндсэн servo */

#BAS_SERVO -г тодорхойлох 4

/ * Мөрний Servo HS-5745-MG */

#тодорхойлох SHL_SERVO 5

/ * Тохой Servo HS-5745-MG */

#тодорхойлох ELB_SERVO 6

/ * Бугуйн servo HS-645MG */

#тодорхойлох WRI_SERVO 7

/ * Бугуй эргүүлэх servo HS-485HB */

#тодорхойлох WRO_SERVO 8

/ * HS-422 шүүрч авах зориулалттай servo */

#тодорхойлох GRI_SERVO 9

/ * урьдчилсан тооцоо */

float hum_sq = HUMERUS*HUMERUS;

float uln_sq = ULNA*ULNA;

int servoSPeed = 10;

// ServoShield servo; // ServoShield объект

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

int loopCounter = 0;

int pulseWidth = 6.6;

int microsecondsToDegrees;

хүчингүй тохиргоо ()

{

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

Цуваа эхлэх (9600);

Serial.println ("Эхлэх");

}

хоосон давталт ()

{

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 нь зөвхөн эерэг байж болно */

// void 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 = radians (grip_angle_d); // тооцоолоход ашиглах радиан дахь атгах өнцөг

/ * Суурь өнцөг ба x, y координатаас радиаль зай */

хөвөх bas_angle_r = atan2 (x, y);

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

/ * rdist бол гарны у координат */

y = rdist;

/ * Бариулын офсетыг атгах өнцөг дээр үндэслэн тооцоолсон */

float grip_off_z = (нүгэл (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) */

хөвөх s_w = (бугуйн_z * бугуйн_z) + (бугуйны_y * бугуйны_y);

хөвөх s_w_sqrt = sqrt (s_w);

/ * s_w газрын өнцөг */

хөвөх a1 = atan2 (бугуйны_z, бугуйны_y);

/ * humerus руу s_w өнцөг */

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

/ * мөрний өнцөг */

float shl_angle_r = a1 + a2;

хөвөх shl_angle_d = градус (shl_angle_r);

/ * тохойн өнцөг */

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

хөвөх elb_angle_d = градус (elb_angle_r);

хөвөх 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);

хөвөх elb_servopulse = 1500.0 - ((elb_angle_d - 90.0) * pulseWidth);

// float wri_servopulse = 1500 + (wri_angle_d * pulseWidth);

// float wri_servopulse = 1500 + (wri_angle_d * pulseWidth);

float wri_servopulse = 1500 - (wri_angle_d * pulseWidth); // jimrd -ээр 2018/2/11 шинэчлэгдсэн - Би нэмэгдлийг хасах болгож өөрчилсөн - энэ код өмнө нь хэн нэгэнд хэрхэн ажиллаж байсан талаар сайн мэдэхгүй байна. Тохойн servo -ийг дээшээ биш харин доошоо харсан 0 градусаар суулгасан байж болох юм.

/ * Серво тохируулах */

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

microsecondsToDegrees = газрын зураг (ftl (bas_servopulse), 544, 2400, 0, 180);

servo1.write (microsecondsToDegrees, servoSpeed); // энэ функцийг ашиглан servo хурдыг тохируулж болно //

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

}

/ * servos -ийг зогсоолын байрлалд шилжүүлэх */

хүчингүй servo_park ()

{

//servos.setposition(BAS_SERVO, 1500);

servo1. бичих (90, 10);

//servos.setposition(SHL_SERVO, 2100);

servo2. бичих (90, 10);

//servos.setposition(ELB_SERVO, 2100);

servo 3. бичих (90, 10);

//servos.setposition(WRI_SERVO, 1800);

servo4. бичих (90, 10);

//servos.setposition(WRO_SERVO, 600);

servo 5. бичих (90, 10);

//servos.setposition(GRI_SERVO, 900);

servo 6. бичих (80, 10);

буцах;

}

хүчингүй тэг_x ()

{

for (double yaxis = 250.0; yaxis <400.0; yaxis += 1) {

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

set_arm (0, сайн, 200.0, 0, 10);

саатал (10);

}

for (double yaxis = 400.0; yaxis> 250.0; yaxis -= 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);

}

}

хоосон тойрог ()

{

#RADIUS 50.0 -ийг тодорхойлох

// хөвөх өнцөг = 0;

хөвөх zaxis, yaxis;

for (хөвөх өнцөг = 0.0; өнцөг <360.0; өнцөг += 1.0) {

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

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

set_arm (0, yaxis, zaxis, 0, 50);

саатал (10);

}

}

Алхам 4: Баримт, асуудал гэх мэт …

1. Би тойрог () програмыг ажиллуулахдаа робот маань тойрог гэхээсээ илүү эллипс хэлбэртэй хөдөлдөг. Би тэдгээрийн нэгийг туршиж үзсэн бөгөөд 1500 микрекунд нь 90 градусынхтай адил биш байв. Үүнийг шийдвэрлэхийн тулд хичээх болно. Алгоритмд буруу зүйл байгаа гэдэгт бүү итгээрэй, гэхдээ миний тохиргоонд. 2018/2/11 шинэчлэх - энэ нь анхны кодын алдаатай холбоотой болохыг олж мэдсэн. Үүнийг ашиглан түүний програм хэрхэн ажилладаг болохыг би ойлгохгүй байна: float wri_servopulse = 1500 - (wri_angle_d * pulseWidth); (анхны код нэмж байсан)

2. set_arm () функц хэрхэн ажилладаг талаар дэлгэрэнгүй мэдээллийг хаанаас олж болох вэ: Олег Мазуровын вэбсайт бүх зүйлийг тайлбарласан эсвэл нэмэлт мэдээлэл авах линкүүдийг оруулсан болно:

3. Хилийн нөхцлийг шалгах ямар нэгэн зүйл байна уу? Үгүй. Миний робот гарыг хүчингүй xyz координатаар дамжуулж өгөхдөө энэ нь муур сунах шиг ийм нуман хэлбэртэй хөдөлгөөнийг хийдэг. Олег гар хөдөлгөөнийг програмчлахад USB ашигладаг хамгийн сүүлийн үеийн програмаа шалгаж байгаа гэдэгт би итгэж байна. Түүний видеог үзээд хамгийн сүүлийн кодын линкийг үзнэ үү.

4. Кодыг цэвэрлэж, микросекунд кодыг устгах хэрэгтэй.