Gebruik Arduino Uno vir XYZ -posisionering van 6 DOF -robotarms: 4 stappe

2024 Outeur: John Day | [email protected]. Laas verander: 2024-01-30 07:26

Hierdie projek handel oor die implementering van 'n kort en relatief maklike Arduino -skets om XYZ -inverse kinematiese posisionering te bied. Ek het 'n 6 servo robotarm gebou, maar as dit by die vind van sagteware kom, was daar nie veel nie, behalwe vir aangepaste programme wat op pasgemaakte servo-skilde werk, soos die SSC-32 (U) of ander programme en programme wat ingewikkeld om te installeer en met die arm te kommunikeer. Toe vind ek Oleg Mazurov se mees uitstekende "Robotic Arm Inverse Kinematics on Arduino", waar hy inverse kinematika in 'n eenvoudige Arduino -skets implementeer.

Ek het twee wysigings aangebring om sy kode aan te pas:

1. Ek het die VarSpeedServo -biblioteek gebruik in plaas van sy persoonlike servoskildbiblioteek, want ek kon dan die snelheid van die servo's beheer en ek hoef nie die servo -skild wat hy gebruik, te gebruik nie. Vir almal wat dit oorweeg om die kode wat hier verskaf word, aan te beveel, raai ek u aan om hierdie VarSpeedServo -biblioteek te gebruik, eerder as die servo.h -biblioteek, sodat u u robotarmbeweging kan vertraag tydens ontwikkeling, of u kan vind dat die arm u onverwags sal steek die gesig of erger, want dit sal teen volle servosnelheid beweeg.

2. Ek gebruik 'n eenvoudige sensor/servoskerm om die servo's aan die Arduino Uno te koppel, maar dit benodig geen spesiale servobiblioteek nie, aangesien dit net die penne van die Arduino gebruik. Dit kos slegs 'n paar dollar, maar dit is nie nodig nie. Dit sorg vir 'n mooi skoon verbinding van die servo's met die Arduino. En ek sal nou nooit weer na die bedrade servo's van die Arduino Uno terugkeer nie. As u hierdie sensor/servo -skild gebruik, moet u 'n klein aanpassing aanbring wat ek hieronder sal uiteensit.

Die kode werk uitstekend en stel u in staat om die arm te gebruik deur 'n enkele funksie te gebruik waarin u die x-, y-, x- en snelheidsparameters deurgee. Byvoorbeeld:

stel_arm (0, 240, 100, 0, 20); // parameters is (x, y, z, gryphoek, servosnelheid)

vertraging (3000); // vertraging is nodig om armtyd na hierdie plek toe te laat beweeg

Kan nie eenvoudiger wees nie. Ek sal die skets hieronder insluit.

Oleg se video is hier: Die beheer van die robotarm met Arduino en USB -muis

Oleg se oorspronklike program, beskrywings en hulpbronne: Oleg's Inverse Kinematics vir Arduino Uno

Ek verstaan nie al die wiskunde agter die roetine nie, maar die goeie ding is dat u nie die kode hoef te gebruik nie. Hoop jy sal dit probeer.

Stap 1: Hardeware -modifikasies

1. Die enigste ding wat nodig is, is dat u servo in die verwagte rigtings draai, wat u kan vereis dat u die montering van u servo's fisies moet omkeer. Gaan na hierdie bladsy om die verwagte servorigting vir basis-, skouer-, elmboog- en pols -servo's te sien:

2. As u die sensorskerm wat ek gebruik, gebruik, moet u een ding daaraan doen: buig die pen wat die 5v van die skild met die Arduino Uno verbind, sodat dit nie aan die Uno -bord kan koppel nie. U wil die eksterne spanning op die skild gebruik om slegs u servo's aan te dryf, nie die Arduino Uno nie, of dit kan die Uno vernietig. om meer as 5v te gebruik om u servo's aan te dryf, maar as u eksterne spanning hoër is as 5 volt, moet u geen 5 volt sensors aan die skerm koppel nie, anders word dit gebraai.

Stap 2: Laai die VarSpeedServo -biblioteek af

U moet hierdie biblioteek gebruik wat die standaard arduino servobiblioteek vervang, omdat dit u toelaat om 'n servosnelheid in die servo -skryfverklaring oor te dra. Die biblioteek is hier geleë:

VarSpeedServo -biblioteek

U kan net die zip -knoppie gebruik, die zip -lêer aflaai en dit dan met die Arduino IDE installeer. Nadat die opdrag in u program geïnstalleer is, sal dit soos volg lyk: servo.write (100, 20);

Die eerste parameter is die hoek en die tweede is die snelheid van die servo van 0 tot 255 (volspoed).

Stap 3: voer hierdie skets uit

Hier is die kompetisieprogram. U moet 'n paar parameters vir u robotarmafmetings verander:

1. BASE_HGT, HUMERUS, ULNA, GRIPPER lengtes in millimeter.

2. Voer u servopennommers in

3. Voer die servo min en max in die aangehegte state in.

4. Probeer dan 'n eenvoudige set_arm () opdrag en dan die zero_x (), line () en sirkel () funksies om te toets. Maak seker dat u servosnelheid laag is die eerste keer dat u hierdie funksies uitvoer om te voorkom dat u u arm en u eie arm beskadig.

Sterkte.

#sluit VarSpeedServo.h in

/ * Servobesturing vir AL5D -arm */

/ * Armafmetings (mm) */

#definieer BASE_HGT 90 // basishoogte

#definieer HUMERUS 100 // skouer-tot-elmboog "been"

#definieer ULNA 135 // elmboog-tot-pols "been"

#define GRIPPER 200 // grijper (insluitend meganisme om pols te draai) lengte"

#define ftl (x) ((x)> = 0? (long) ((x) +0.5):(long) ((x) -0.5)) // float to long conversion

/ * Servo name/nommers *

* Basisservo HS-485HB */

#definieer BAS_SERVO 4

/ * Skouerservo HS-5745-MG */

#definieer SHL_SERVO 5

/ * Elmboog Servo HS-5745-MG */

#definieer ELB_SERVO 6

/ * Pols servo HS-645MG */

#definieer WRI_SERVO 7

/ * Polsdraai servo HS-485HB */

#definieer WRO_SERVO 8

/ * Gripper servo HS-422 */

#definieer GRI_SERVO 9

/ * vooraf berekeninge */

float hum_sq = HUMERUS*HUMERUS;

float uln_sq = ULNA*ULNA;

int servoSPeed = 10;

// ServoShield servo's; // ServoShield -voorwerp

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

int loopCounter = 0;

int pulseWidth = 6.6;

int microsecondsToDegrees;

leemte opstelling ()

{

servo1.aanheg (BAS_SERVO, 544, 2400);

servo2.aanheg (SHL_SERVO, 544, 2400);

servo3.aanheg (ELB_SERVO, 544, 2400);

servo4.aanheg (WRI_SERVO, 544, 2400);

servo5.aanheg (WRO_SERVO, 544, 2400);

servo6.aanheg (GRI_SERVO, 544, 2400);

vertraging (5500);

//servos.start (); // Begin die servoskild

servo_park ();

vertraging (4000);

Serial.begin (9600);

Serial.println ("Begin");

}

leemte lus ()

{

loopCounter += 1;

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

// vertraging (7000);

// zero_x ();

// reël ();

// sirkel ();

vertraging (4000);

as (loopCounter> 1) {

servo_park ();

// set_arm (0, 0, 0, 0, 10); // park

vertraging (5000);

uitgang (0); } // pouse program - druk reset om voort te gaan

// uitgang (0);

}

/ * armposisioneringsroetine met inverse kinematika */

/* z is die hoogte, y is die afstand van die middelpunt na buite, x is van kant tot kant. y, z kan slegs positief wees */

// 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 = radiale (grip_angle_d); // gryphoek in radiale vir gebruik in berekeninge

/ * Basishoek en radiale afstand van x, y koördinate */

float bas_angle_r = atan2 (x, y);

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

/ * rdist is y koördinaat vir die arm */

y = rdist;

/ * Grip -offsets bereken op grond van die greephoek */

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

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

/ * Polsposisie */

float pols_z = (z - grip_off_z) - BASE_HGT;

float pols_y = y - grip_off_y;

/ * Skouer tot pols afstand (AKA sw) */

float s_w = (pols_z * pols_z) + (pols_y * pols_y);

float s_w_sqrt = sqrt (s_w);

/ * s_w hoek tot grond */

float a1 = atan2 (pols_z, pols_y);

/ * s_w hoek na humerus */

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

/ * skouerhoek */

dryf shl_angle_r = a1 + a2;

dryf shl_angle_d = grade (shl_angle_r);

/ * elmbooghoek */

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

float elb_angle_d = grade (elb_angle_r);

float elb_angle_dn = - (180.0 - elb_angle_d);

/ * polshoek */

float wri_angle_d = (grip_angle_d - elb_angle_dn) - shl_angle_d;

/ * Servopulse */

float bas_servopulse = 1500.0 - ((grade (bas_angle_r)) * pulswydte);

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); // opgedateer 2018/2/11 deur jimrd - ek het die pluspunt na 'n minus verander - nie seker hoe hierdie kode voorheen vir iemand gewerk het nie. Dit kan wees dat die elmboog -servo gemonteer is met 0 grade na onder eerder as omhoog.

/ * Stel servo's */

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

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

servo1.write (microsecondsToDegrees, servoSpeed); // gebruik hierdie funksie sodat u servosnelheid kan stel //

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

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

servo2.write (microsecondsToDegrees, servoSpeed);

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

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

servo3.write (microsecondsToDegrees, servoSpeed);

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

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

servo4.write (microsecondsToDegrees, servoSpeed);

}

/ * skuif servo's na parkeerposisie */

leegte 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. skryf (90, 10);

//servos.setposition(WRI_SERVO, 1800);

servo4.write (90, 10);

//servos.setposition(WRO_SERVO, 600);

servo5. skryf (90, 10);

//servos.setposition(GRI_SERVO, 900);

servo6. skryf (80, 10);

terugkeer;

}

leemte zero_x ()

{

vir (dubbel yaxis = 250,0; yaxis <400,0; yaxis += 1) {

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

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

vertraging (10);

}

vir (dubbele yaxis = 400,0; yaxis> 250,0; yaxis -= 1) {

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

vertraging (10);

}

}

/ * beweeg arm in 'n reguit lyn */

leegte lyn ()

{

vir (dubbel xaxis = -100,0; xaxis <100,0; xaxis += 0,5) {

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

vertraging (10);

}

vir (float xaxis = 100,0; xaxis> -100,0; xaxis -= 0,5) {

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

vertraging (10);

}

}

leegte sirkel ()

{

#definieer RADIUS 50.0

// dryfhoek = 0;

float zaxis, yaxis;

vir (vlotterhoek = 0,0; hoek <360,0; hoek += 1,0) {

yaxis = RADIUS * sin (radiale (hoek)) + 300;

zaxis = RADIUS * cos (radiale (hoek)) + 200;

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

vertraging (10);

}

}

Stap 4: Feite, kwessies en dies meer …

1. As ek met die sirkel () subroutine loop, beweeg my robot meer in 'n elliptiese vorm as in 'n sirkel. Ek dink dit is omdat my servo's nie gekalibreer is nie. Ek het een daarvan getoets en 1500 mikrosekondes was nie dieselfde as 90 grade nie. Sal hieraan werk om 'n oplossing te vind. Glo nie daar is iets fout met die algoritme nie, maar eerder met my instellings. Werk 2018/2/11 op - het net agtergekom dat dit te wyte is aan die fout in die oorspronklike kode. Ek sien nie hoe sy program werk nie Vaste kode met behulp van hierdie: float wri_servopulse = 1500 - (wri_angle_d * pulseWidth); (oorspronklike kode is bygevoeg)

2. Waar kan ek meer inligting kry oor hoe die set_arm () funksie werk: Oleg Mazurov se webwerf verduidelik alles of bied skakels vir meer inligting:

3. Is daar enige randvoorwaardekontrole? Nee. As my robotarm verby is, voer 'n ongeldige xyz -koördinaat hierdie snaakse boogbeweging uit soos 'n kat wat rek. Ek glo dat Oleg 'n bietjie kyk na sy nuutste program wat 'n USB gebruik om die armbewegings te programmeer. Sien sy video en skakel na sy nuutste kode.

4. Kode moet skoongemaak word en mikrosekonde -kode kan verwyder word.