การใช้ Arduino Uno สำหรับการวางตำแหน่ง XYZ ของแขนหุ่นยนต์ 6 DOF: 4 ขั้นตอน

2024 ผู้เขียน: John Day | [email protected]. แก้ไขล่าสุด: 2024-01-30 13:06

โปรเจ็กต์นี้เกี่ยวกับการนำร่าง Arduino ที่สั้นและค่อนข้างง่ายไปใช้ในการจัดตำแหน่งจลนศาสตร์ผกผันของ XYZ ฉันได้สร้างแขนหุ่นยนต์เซอร์โว 6 ตัวแล้ว แต่เมื่อต้องหาซอฟต์แวร์เพื่อใช้งาน ไม่มีอะไรมากนอกจากโปรแกรมแบบกำหนดเองที่ทำงานบนเซอร์โวชิลด์แบบกำหนดเอง เช่น SSC-32(U) หรือโปรแกรมและแอปอื่นๆ ซับซ้อนในการติดตั้งและสื่อสารกับแขน จากนั้นฉันก็พบว่า "Robotic Arm Inverse Kinematics บน Arduino" ที่ยอดเยี่ยมที่สุดของ Oleg Mazurov ซึ่งเขาใช้จลนศาสตร์ผกผันในแบบร่าง 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); // จำเป็นต้องมีการหน่วงเวลาเพื่อให้เวลาแขนเคลื่อนที่ไปยังตำแหน่งนี้

ไม่สามารถง่ายกว่านี้ ฉันจะรวมร่างด้านล่าง

วิดีโอของ Oleg อยู่ที่นี่: การควบคุม Robotic Arm ด้วย Arduino และ USB Mouse

โปรแกรม คำอธิบาย และทรัพยากรดั้งเดิมของ Oleg: Inverse Kinematics ของ Oleg สำหรับ Arduino Uno

ฉันไม่เข้าใจคณิตศาสตร์ทั้งหมดที่อยู่เบื้องหลังกิจวัตร แต่ข้อดีคือคุณไม่ต้องใช้รหัส หวังว่าคุณจะลองดู

ขั้นตอนที่ 1: การปรับเปลี่ยนฮาร์ดแวร์

1. สิ่งเดียวที่จำเป็นคือเซอร์โวของคุณหมุนไปในทิศทางที่คาดไว้ ซึ่งอาจทำให้คุณต้องย้อนกลับการติดตั้งเซอร์โวของคุณ ไปที่หน้านี้เพื่อดูทิศทางเซอร์โวที่คาดหวังสำหรับเซอร์โวฐาน ไหล่ ข้อศอก และข้อมือ:

2. หากคุณใช้ชิลด์เซ็นเซอร์ที่ฉันใช้อยู่ คุณต้องทำสิ่งหนึ่งกับมัน: งอพินที่เชื่อมต่อ 5v จากชิลด์กับ Arduino Uno ให้พ้นทางเพื่อไม่ให้เชื่อมต่อกับบอร์ด Uno คุณต้องการใช้แรงดันไฟภายนอกบนชิลด์เพื่อจ่ายไฟให้กับเซอร์โวของคุณเท่านั้น ไม่ใช่ Arduino Uno หรือมันอาจทำลาย Uno ได้ ฉันรู้ในขณะที่ฉันเผาบอร์ด Uno สองแผงเมื่อแรงดันไฟฟ้าภายนอกของฉันอยู่ที่ 6 โวลต์มากกว่า 5 วิธีนี้ช่วยให้คุณ หากต้องการใช้ที่สูงกว่า 5v เพื่อจ่ายไฟให้กับเซอร์โวของคุณ แต่ถ้าแรงดันไฟฟ้าภายนอกของคุณสูงกว่า 5 โวลต์ อย่าเชื่อมต่อเซ็นเซอร์ 5 โวลต์ใดๆ เข้ากับแผงป้องกัน มิฉะนั้น เซ็นเซอร์จะถูกทอดทิ้ง

ขั้นตอนที่ 2: ดาวน์โหลด VarSpeedServo Library

คุณต้องใช้ไลบรารีนี้แทนที่ไลบรารีเซอร์โว Arduino มาตรฐานเนื่องจากช่วยให้คุณสามารถส่งความเร็วเซอร์โวไปยังคำสั่งการเขียนเซอร์โวได้ ห้องสมุดตั้งอยู่ที่นี่:

ห้องสมุด VarSpeedServo

คุณสามารถใช้ปุ่ม zip ดาวน์โหลดไฟล์ zip แล้วติดตั้งด้วย Arduino IDE เมื่อติดตั้งคำสั่งในโปรแกรมของคุณจะมีลักษณะดังนี้: servo.write(100, 20);

พารามิเตอร์แรกคือมุม และพารามิเตอร์ที่สองคือความเร็วของเซอร์โวตั้งแต่ 0 ถึง 255 (ความเร็วเต็มที่)

ขั้นตอนที่ 3: เรียกใช้ Sketch นี้

นี่คือโปรแกรมการแข่งขัน คุณต้องแก้ไขพารามิเตอร์สองสามตัวสำหรับขนาดแขนหุ่นยนต์ของคุณ:

1. BASE_HGT, HUMERUS, ULNA, GRIPPER ความยาวหน่วยเป็นมิลลิเมตร

2. ป้อนหมายเลขพินเซอร์โวของคุณ

3. ป้อนเซอร์โวต่ำสุดและสูงสุดในคำสั่งแนบ

4. จากนั้นลองใช้คำสั่ง set_arm() อย่างง่าย จากนั้นใช้ฟังก์ชัน zero_x(), line() และ circle() สำหรับการทดสอบ ตรวจสอบให้แน่ใจว่าความเร็วเซอร์โวของคุณต่ำในครั้งแรกที่คุณเรียกใช้ฟังก์ชันเหล่านี้ เพื่อป้องกันความเสียหายที่แขนและแขนของคุณเอง

ขอให้โชคดี.

#include VarSpeedServo.h

/* การควบคุมเซอร์โวสำหรับแขน AL5D */

/* ขนาดแขน(mm) */

#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)) // การแปลงจากโฟลตเป็นแบบยาว

/* ชื่อ/หมายเลขเซอร์โว *

* ฐานเซอร์โว 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 เซอร์โว; //ServoShield วัตถุ

VarSpeedServo เซอร์โว1, เซอร์โว2, เซอร์โว3, เซอร์โว4, เซอร์โว5, เซอร์โว6;

int loopCounter=0;

int ชีพจรความกว้าง = 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);

Serial.begin(9600);

Serial.println("เริ่ม");

}

วงเป็นโมฆะ ()

{

loopCounter +=1;

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

// ล่าช้า (7000);

//zero_x();

//ไลน์();

//วงกลม();

ล่าช้า(4000);

ถ้า (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)

เป็นโมฆะ 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 = (บาป (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)) * pulseWidth);

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

float 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); // อัปเดต 2018/2/11 โดย jimrd - ฉันเปลี่ยนค่าบวกเป็นลบ - ไม่แน่ใจว่าโค้ดนี้ใช้ได้กับทุกคนมาก่อนอย่างไร อาจเป็นไปได้ว่าข้อศอกเซอร์โวถูกติดตั้งโดยหัน 0 องศาแทนที่จะขึ้น

/* ตั้งค่าเซอร์โว */

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

microsecondsToDegrees = แผนที่ (ftl (bas_servopulse), 544, 2400, 0, 180);

servo1.write (ไมโครวินาทีToDegrees, servoSpeed); // ใช้ฟังก์ชันนี้เพื่อตั้งค่าความเร็วเซอร์โว //

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

microsecondsToDegrees = แผนที่ (ftl(shl_servopulse), 544, 2400, 0, 180);

servo2.write (ไมโครวินาทีToDegrees, servoSpeed);

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

microsecondsToDegrees = แผนที่ (ftl(elb_servopulse), 544, 2400, 0, 180);

servo3.write (ไมโครวินาทีToDegrees, servoSpeed);

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

microsecondsToDegrees = แผนที่ (ftl(wri_servopulse), 544, 2400, 0, 180);

servo4.write (ไมโครวินาทีToDegrees, servoSpeed);

}

/* ย้ายเซอร์โวไปที่ตำแหน่งจอดรถ */

เป็นโมฆะ 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);

กลับ;

}

เป็นโมฆะ zero_x()

{

สำหรับ (แกนคู่ = 250.0; yaxis <400.0; yaxis += 1) {

Serial.print(" yaxis=: ");Serial.println(yaxis); อนุกรม.print(" yaxis=: ");

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

ล่าช้า(10);

}

สำหรับ (แกนคู่ = 400.0; yaxis > 250.0; yaxis -= 1) {

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

ล่าช้า(10);

}

}

/* ขยับแขนเป็นเส้นตรง */

เส้นเป็นโมฆะ ()

{

สำหรับ (xaxis คู่ = -100.0; xaxis < 100.0; xaxis += 0.5) {

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

ล่าช้า(10);

}

สำหรับ (xaxis ลอย = 100.0; xaxis > -100.0; xaxis -= 0.5) {

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

ล่าช้า(10);

}

}

วงกลมว่างเปล่า ()

{

#กำหนดรัศมี 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() หุ่นยนต์ของฉันจะเคลื่อนที่เป็นวงรีมากกว่าวงกลม ฉันคิดว่านั่นเป็นเพราะเซอร์โวของฉันไม่ได้รับการปรับเทียบ ฉันทดสอบหนึ่งในนั้นและ 1,500 ไมโครวินาทีไม่เหมือนกับ 90 องศา จะทำงานนี้เพื่อพยายามหาทางแก้ไข อย่าเชื่อว่ามีอะไรผิดปกติกับอัลกอริทึม แต่กับการตั้งค่าของฉัน อัปเดต 2018/2/11 - เพิ่งค้นพบว่าเกิดจากข้อผิดพลาดในรหัสดั้งเดิม ฉันไม่เห็นว่าโปรแกรมของเขาทำงานอย่างไร รหัสคงที่โดยใช้สิ่งนี้: float wri_servopulse = 1500 - (wri_angle_d * pulseWidth); (รหัสเดิมถูกเพิ่ม)

2. ฉันจะหาข้อมูลเพิ่มเติมเกี่ยวกับวิธีการทำงานของฟังก์ชัน set_arm() ได้ที่ไหน: เว็บไซต์ของ Oleg Mazurov อธิบายทุกอย่างหรือให้ลิงก์สำหรับข้อมูลเพิ่มเติม:

3. มีการตรวจสอบเงื่อนไขขอบเขตหรือไม่? ไม่ เมื่อแขนหุ่นยนต์ของฉันถูกส่งผ่านพิกัด xyz ที่ไม่ถูกต้อง มันจะเคลื่อนไหวแบบโค้งอย่างตลกๆ เหมือนกับการยืดเส้นยืดสายของแมว ฉันเชื่อว่าโอเล็กกำลังตรวจสอบโปรแกรมล่าสุดของเขาที่ใช้ USB เพื่อตั้งโปรแกรมการเคลื่อนไหวของแขน ดูวิดีโอของเขาและลิงก์ไปยังรหัสล่าสุดของเขา

4. ต้องล้างรหัสและรหัสไมโครวินาทีก็สามารถทำได้