Siv Arduino Uno rau XYZ Txoj Haujlwm ntawm 6 DOF Robotic Arm: 4 Kauj Ruam

2024 Tus sau: John Day | [email protected]. Kawg hloov kho: 2024-01-30 09:28

Txoj haujlwm no yog hais txog kev siv luv thiab yooj yim Arduino kos duab los muab XYZ inverse kinematic positioning. Kuv tau tsim 6 servo robotic caj npab tab sis thaum nws los txog nrhiav software los khiav nws, tsis muaj ntau qhov tawm tshwj tsis yog rau cov kev pabcuam tshwj xeeb uas khiav ntawm kev cai tiv thaiv kev tiv thaiv zoo li SSC-32 (U) lossis lwm cov haujlwm thiab apps uas tau ua nyuaj rau nruab thiab sib tham nrog caj npab. Tom qab ntawd kuv pom Oleg Mazurov qhov zoo tshaj plaws "Robotic Arm Inverse Kinematics on Arduino" qhov uas nws tau siv cov kinematics ntxeev hauv qhov yooj yim Arduino kos duab.

Kuv tau ua ob qho kev hloov kho kom haum nws cov cai:

1. Kuv tau siv VarSpeedServo lub tsev qiv ntawv los ntawm nws qhov kev cai servo shield lub tsev qiv ntawv vim kuv tuaj yeem tswj hwm qhov nrawm ntawm servos thiab kuv yuav tsis tas siv daim ntaub thaiv uas nws siv. Rau ib qho twg txiav txim siab ua tus lej muab rau ntawm no kuv pom zoo tias koj siv lub tsev qiv ntawv VarSpeedServo no, tsis yog lub tsev qiv ntawv servo.h, yog li koj tuaj yeem qeeb koj cov neeg hlau caj npab txav thaum lub sijhawm txhim kho lossis koj yuav pom tias txhais caj npab yuav ua rau koj poob nthav lub ntsej muag lossis tsis zoo vim tias nws yuav txav mus los ntawm qhov kev pabcuam puv ntoob.

2. Kuv siv lub sensor yooj yim/daim thaiv npog los txuas cov servos rau Arduino Uno tab sis nws yuav tsum tsis muaj lub tsev qiv ntawv tshwj xeeb tshwj xeeb vim nws tsuas yog siv Arduino tus pin. Nws tsuas yog them nqi ob peb bucks tab sis nws tsis xav tau. Nws ua rau kev sib txuas huv huv ntawm cov servos rau Arduino. Thiab kuv yuav tsis rov qab mus rau qhov ua haujlwm nyuaj rau Arduino Uno tam sim no. Yog tias koj siv lub ntsuas hluav taws xob/daim npav servo no koj yuav tsum tau hloov kho me me uas kuv yuav piav qhia hauv qab no.

Txoj cai ua haujlwm tau zoo thiab tso cai rau koj ua haujlwm ntawm caj npab los ntawm kev siv ua haujlwm ib leeg uas koj dhau qhov x, y, x thiab ntsuas tsis nrawm. Piv txwv li:

set_arm (0, 240, 100, 0, 20); // tsis yog (x, y, z, gripper angle, servo speed)

ncua (3000); // ncua sijhawm yuav tsum tau tso cai rau lub sijhawm npab txav mus rau qhov chaw no

Yuav tsis yooj yim dua. Kuv yuav suav nrog cov duab kos hauv qab no.

Oleg cov vis dis aus nyob ntawm no: Tswj Robotic Arm nrog Arduino thiab USB Mouse

Oleg thawj qhov haujlwm, piav qhia, thiab peev txheej: Oleg's Inverse Kinematics rau Arduino Uno

Kuv tsis nkag siab txhua qhov lej tom qab ua ntu zus tab sis qhov zoo yog koj tsis tas yuav siv txoj cai. Vam tias koj yuav muab nws sim.

Kauj ruam 1: Kho vajtse Modifcations

1. Qhov tsuas yog qhov xav tau yog tias koj lub servo tig rau hauv cov lus qhia uas xav tau uas tuaj yeem xav kom koj lub cev thim rov qab ntawm koj cov servos. Mus rau nplooj ntawv no kom pom qhov kev cia siab ua haujlwm rau lub hauv paus, xub pwg, lub luj tshib, thiab lub dab teg ua haujlwm:

2. Yog tias koj siv lub ntsuas qhov ntsuas uas kuv tab tom siv koj yuav tsum ua ib yam rau nws: khoov tus pin uas txuas 5v los ntawm daim ntaub thaiv mus rau Arduino Uno tawm ntawm txoj kev kom nws tsis txuas rau Uno board. Koj xav siv qhov hluav taws xob sab nraud ntawm daim ntaub thaiv kom muaj zog tsuas yog koj li servos, tsis yog Arduino Uno lossis nws tuaj yeem rhuav tshem Uno, Kuv paub zoo li kuv tau hlawv ob lub Uno boards thaum kuv qhov hluav taws xob sab nraud yog 6 volts ntau dua li 5. Qhov no tso cai rau koj siv siab dua 5v txhawm rau txhawb koj lub servos tab sis yog tias koj qhov hluav taws xob sab nraud siab dua 5 volts ces tsis txhob txuas ib qho 5 volt sensors mus rau daim ntaub thaiv lossis lawv yuav kib.

Kauj Ruam 2: Rub tawm VarSpeedServo Library

Koj yuav tsum siv lub tsev qiv ntawv no uas hloov pauv tus qauv arduino servo lub tsev qiv ntawv vim nws tso cai rau koj kom dhau qhov nrawm nrawm rau hauv servo sau nqe lus. Lub tsev qiv ntawv nyob ntawm no:

VarSpeedServo Library

Koj tsuas tuaj yeem siv khawm zip, rub tawm cov ntawv zip thiab tom qab ntawd nruab nws nrog Arduino IDE. Thaum teeb tsa cov lus txib hauv koj qhov program yuav zoo li: servo.write (100, 20);

Thawj qhov ntsuas yog lub kaum sab xis thiab qhov thib ob yog qhov nrawm ntawm servo los ntawm 0 txog 255 (tag nrho nrawm).

Kauj Ruam 3: Khiav Qhov Sketch no

Nov yog qhov program sib tw. Koj yuav tsum hloov kho ob peb yam rau koj cov neeg hlau caj npab qhov ntev:

1. BASE_HGT, HUMERUS, ULNA, GRIPPER qhov ntev hauv mm.

2. Sau koj tus lej tus lej servo

3. Nkag mus rau servo min thiab max hauv kab lus txuas.

4. Tom qab ntawd sim ua qhov yooj yim set_arm () hais kom ua thiab tom qab ntawd xoom_x (), kab () thiab lub voj voog () ua haujlwm rau kev sim. Nco ntsoov tias koj lub servo nrawm qis thawj zaug koj ua cov haujlwm no txhawm rau tiv thaiv koj txhais caj npab thiab koj txhais tes.

Hmoov zoo.

#suav nrog VarSpeedServo.h

/ * Servo tswj rau AL5D caj npab */

/ * Caj npab qhov ntev (hli) */

#define BASE_HGT 90 // qhov siab

#define HUMERUS 100 // xub pwg-rau-luj tshib "pob txha"

#define ULNA 135 // lub luj tshib-rau-dab teg "pob txha"

#define GRIPPER 200 // tuav (suav nrog lub luag haujlwm hnyav lub dab teg tig lub cav) qhov ntev"

#define ftl (x) ((x)> = 0? (ntev) ((x) +0.5):(ntev) ((x) -0.5)) // ntab rau kev hloov pauv ntev

/ * Servo cov npe/tus lej *

* Base servo HS-485HB */

#define BAS_SERVO 4

/ * Lub xub pwg Servo HS-5745-MG */

#define SHL_SERVO 5

/ * Luj Tshib Servo HS-5745-MG */

#define ELB_SERVO 6

/ * Lub dab teg servo HS-645MG */

#define WRI_SERVO 7

/ * Lub dab teg tig servo HS-485HB */

#define WRO_SERVO 8

/ * Gripper servo HS-422 */

#define GRI_SERVO 9

/ * kev suav ua ntej */

ntab hum_sq = HUMERUS*HUMERUS;

ntab uln_sq = ULNA*ULNA;

int servoSPeed = 10; cov.

// ServoShield servos; // ServoShield kwv

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

int loopCounter = 0;

pulseWidth = 6.6;

koj microsecondsToDegrees;

void teeb tsa ()

{

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

ncua (5500);

//servos.start (); // Pib lub servo shield

servo_park (); cov.

ncua (4000);

Serial.begin (9600);

Serial.println ("Pib");

}

void lub voj ()

{

loopCounter += 1;

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

// ncua (7000);

// zero_x ();

// kab ();

// voj voog ();

ncua (4000);

yog (loopCounter> 1) {

servo_park (); cov.

// set_arm (0, 0, 0, 0, 10); // chaw ua si

ncua (5000);

tawm (0); } // ncua txoj haujlwm - ntaus pib dua kom txuas ntxiv

// tawm (0);

}

/ * caj npab tso txoj haujlwm niaj hnub siv kinematics rov qab */

/* z yog qhov siab, y yog nyob deb ntawm lub hauv paus nruab nrab tawm, x yog sab rau sab. y, z tsuas tuaj yeem ua tau zoo */

// void set_arm (uint16_t x, uint16_t y, uint16_t z, uint16_t tuav_angle)

void set_arm (ntab x, ntab y, ntab z, ntab tuav_angle_d, int servoSpeed)

{

ntab grip_angle_r = radians (grip_angle_d); // tuav lub kaum hauv radians rau siv hauv kev suav

/ * Lub kaum sab xis thiab qhov nrug deb ntawm x, y tswj hwm */

ntab bas_angle_r = atan2 (x, y);

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

/ * rdist yog y koom tes rau caj npab */

y = rdist;

/ * Grip offsets xam raws lub kaum sab xis tuav */

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

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

/ * Txoj hauj lwm ntawm lub dab teg */

ntab dab teg_z = (z - tuav_off_z) - BASE_HGT;

ntab dab teg_y = y - tuav_off_y;

/ * Lub xub pwg mus rau lub dab teg nrug (AKA sw) */

ntab s_w = (dab teg_z * dab teg_z) + (dab teg_y * dab teg_y);

ntab s_w_sqrt = sqrt (s_w);

/ * s_w kaum rau hauv av */

ntab a1 = atan2 (dab teg_z, dab teg_y);

/ * s_w kaum rau humerus */

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

/ * lub xub pwg nyom */

ntab shl_angle_r = a1 + a2;

ntab shl_angle_d = degrees (shl_angle_r);

/ * lub luj tshib lub kaum ntse ntse */

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

ntab elb_angle_d = degrees (elb_angle_r);

ntab elb_angle_dn = - (180.0 - elb_angle_d);

/ * lub kaum dab teg */

ntab wri_angle_d = (grip_angle_d - elb_angle_dn) - shl_angle_d;

/ * Servo mem tes */

ntab bas_servopulse = 1500.0 - ((degrees (bas_angle_r)) * pulseWidth);

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

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

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

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

ntab wri_servopulse = 1500 - (wri_angle_d * pulseWidth); // hloov tshiab 2018/2/11 los ntawm jimrd - Kuv hloov pauv ntxiv rau rho tawm - tsis paub tseeb tias txoj cai no ua haujlwm rau leej twg ua ntej. Yuav yog tias lub luj tshib servo tau teeb tsa nrog 0 qib tig rov los es tsis nce.

/ * Teeb tsa servos */

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

microsecondsToDegrees = daim ntawv qhia (ftl (bas_servopulse), 544, 2400, 0, 180);

servo1.write (microsecondsToDegrees, servoSpeed); // siv txoj haujlwm no kom koj tuaj yeem teeb tsa servo nrawm //

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

microsecondsToDegrees = daim ntawv qhia (ftl (shl_servopulse), 544, 2400, 0, 180);

servo2.write (microsecondsToDegrees, servoSpeed);

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

microsecondsToDegrees = daim ntawv qhia (ftl (elb_servopulse), 544, 2400, 0, 180);

servo3.write (microsecondsToDegrees, servoSpeed);

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

microsecondsToDegrees = daim ntawv qhia (ftl (wri_servopulse), 544, 2400, 0, 180);

servo4.write (microsecondsToDegrees, servoSpeed);

}

/ * txav servos mus rau qhov chaw nres tsheb */

tsis muaj dab tsi 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);

rov qab;

}

void zero_x ()

{

rau (ob chav yaxis = 250.0; yaxis <400.0; yaxis += 1) {

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

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

ncua (10);

}

rau (ob yaxis = 400.0; yaxis> 250.0; yaxis -= 1) {

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

ncua (10);

}

}

/ * txav caj npab hauv kab ncaj */

void kab ()

{

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

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

ncua (10);

}

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

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

ncua (10);

}

}

lub voj void ()

{

#define RADIUS 50.0

// ntab kaum = 0;

ntab zaxis, yaxis;

rau (ntab kaum = 0.0; kaum sab xis <360.0; lub kaum sab xis += 1.0) {

yaxis = RADIUS * sin (radians (kaum sab xis)) + 300;

zaxis = RADIUS * cos (radians (lub kaum sab xis)) + 200;

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

ncua (10);

}

}

Kauj Ruam 4: Cov Lus Tseeb, Teeb Meem thiab Zoo Li …

1. Thaum kuv khiav lub voj voos () subroutine kuv tus neeg hlau txav ntau dua nyob rau hauv lub elliptical zoo li lub voj voog Kuv xav tias yog vim kuv cov servos tsis raug ntsuas. Kuv sim ib qho ntawm lawv thiab 1500 microseconds tsis zoo ib yam li 90 degrees. Yuav ua haujlwm ntawm qhov no los sim thiab nrhiav kev daws teeb meem. Tsis txhob ntseeg tias muaj dab tsi tsis raug nrog lub algorithm tab sis theej nrog kuv cov chaw. Hloov tshiab 2018/2/11 - tsuas yog pom qhov no yog vim muaj qhov ua yuam kev hauv tus lej qub. Kuv tsis pom yuav ua li cas nws txoj haujlwm ua haujlwm Kho tus lej siv qhov no: ntab wri_servopulse = 1500 - (wri_angle_d * pulseWidth); (thawj qhov chaws tau ntxiv)

2. Kuv tuaj yeem nrhiav cov ntaub ntawv ntau ntxiv txog qhov set_arm () ua haujlwm li cas: Oleg Mazurov lub vev xaib piav qhia txhua yam lossis muab kev txuas rau cov ntaub ntawv ntxiv:

3. Puas muaj kev txheeb xyuas tus ciam av? Tsis yog. Thaum kuv txhais tes neeg hlau hla dhau qhov tsis raug xyz ua haujlwm nws ua qhov kev lom zem no ntawm kev txav txav zoo li miv ncab. Kuv ntseeg tias Oleg ua qee qhov kev tshuaj xyuas hauv nws qhov haujlwm tshiab uas siv USB los ua haujlwm rau caj npab txav mus los. Saib nws cov vis dis aus thiab txuas rau nws qhov chaws kawg.

4. Txoj cai yuav tsum tau ntxuav thiab microsecond code tuaj yeem ua tiav nrog.