Conducerea roboților mobili şi a

manipulatoarelor robotice

Proiect

Profesor coordonator:

Conf.dr.ing.Șolea Răzvan

Studenți:

Rusu George
Stăncescu Sorin
Zaharia Adrian
 Introducere
Proiectul nostru are în vedere manipularea unui braț robotic utilizând mediul
Matlab.
În această lucrare vom parcurge pas cu pas metode de la cele mai simple
la cinematica inversă, folosită și la nivel industrial.

Documentație

Robotul nostru are două tipuri de motoare: RX10, RX28, RX64. Pentru a
afla mai multe despre aceste două modele, și totodata, pentru a descărca
eventualele drivere sau programe ce ar putea fi utilizate în manipularea
brațului am folosit site-ul http://www.robotis.com.

De pe site-ul Robotis am descărcat softul Dinamixel Wizard 2.0, și cu

ajutorul aplicației RoboPlus Web(v1.1.3.0) am putut identifica pe COM3 toate
cele 8 motorașe.
În această aplicație am testat fiecare motor verificând limitele unghiurilor.
Programarea în Matlab

Pentru a lucra în Matlab am avut nevoie de următoarele headere. Din

folderul Robotis am adăugat în Matlab, cu Set Path, folderul USB2Dynamixel.
Iar din folderul DynamixelSDK-master am adăugat tot.
 Tot pentru Matlab a trebuit să descărcăm un Add-on numit MinGW-
W64(este nevoie de cont Matlab).

Cu ajutorul următoarelor linii de comandă se conecteaza PC la brațul

robotic.
id=1;
loadlibrary('dynamixel','dynamixel.h');
libfunctions('dynamixel');
port = 3;
BaudRate = 1;
res =
calllib('dynamixel','dxl_initialize',port,BaudRate);
if res == 1
disp('Succeed to open USB2Dynamixel!');

StatusError=calllib('dynamixel','dxl_get_result');
Cu ajutorul următoarelor comenzi am reușit să mișcăm brațul.

calllib('dynamixel','dxl_write_word',nr motor,32,viteza);
calllib('dynamixel','dxl_write_word',nr motor,30,unghiul*1024/300);
pause(5);

Nr motor - de la 0 la 7.
Viteza - între 0-1023 în sensul ceasului și 1024-2047 invers ceasului. 0 fiind
viteza maximă (nerecomandat).

Cinematica inversă

Am ales 3 motorașe pentru a putea mișca brațul în plan 3D.

Motoarele cu ID 0,3,5.

Am simplificat modelul brațului, utilizând doar 3 motoare pentru a putea mișca

brațul în spațiul 3D.

Am făcut următoarele măsurări:

De la bază la motorul 2 (l3).
De la motorul 2 la motorul 3 (l1).
De la motorul 3 la vârf (l2).

I-am dat brațului o poziție de start.

calllib('dynamixel','dxl_write_word',7,32,30);
calllib('dynamixel','dxl_write_word',7,30,290*1024/300);
pause(5);

calllib('dynamixel','dxl_write_word',7,32,30);
calllib('dynamixel','dxl_write_word',7,30,100*1024/300);
pause(5);

calllib('dynamixel','dxl_write_word',7,32,30);
calllib('dynamixel','dxl_write_word',7,30,190*1024/300);
pause(5);

calllib('dynamixel','dxl_write_word',1,32,25);
calllib('dynamixel','dxl_write_word',1,30,150*1024/300);
pause(0)

calllib('dynamixel','dxl_write_word',3,32,35);
calllib('dynamixel','dxl_write_word',3,30,230*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',0,32,30);
calllib('dynamixel','dxl_write_word',0,30,150*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',2,32,25);
calllib('dynamixel','dxl_write_word',2,30,150*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',4,32,35);
calllib('dynamixel','dxl_write_word',4,30,150*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',5,32,30);
calllib('dynamixel','dxl_write_word',5,30,150*1024/300);
pause(5);

Această poziție ne ajuta ca motoarele pe care nu le vom folosi pentru

modificarea poziției în spațiu să rămână angrenate (în tensiune).

În funcție de poziția vârfului dorită, latura S va avea dimensiune diferită și

va fi poziționată diferit pe axa verticală.

Latura S este esențială pentru a afla unghiurile triunghiului imaginar

format de către brațele motorului, (l1,l2,S).

d este distanța de la axa verticală până la vârf care se poate afla din
datele introduse, mai exact, coordonatele x și y, d fiind ipotenuza în triunghiul
cu laturile ox, oy, d.

S este latura imaginară care pleacă de la motorul 2 până la vârf.

Următorul pas este să aflăm poziția celor 3 motoare utilizănd datele

calculate anterior, am notat cu a - latura 2, b - latura s, c - latura 1. Și am
calculat unghiurile opuse laturilor.

cosa =(b^2 + c^2 - a^2) / (2*b*c);

cosb = (a^2 + c^2 - b^2) / (2*a*c);
 Mai departe am notat cu c1,c2,c3 valorile ce le vom folosi în Matlab
pentru cele 3 motoare.
c1 = sin(d/c)*180/pi;

aux1 = acos(cosa);
aux2 = aux1 * 180/pi;
c2 = aux2 + alfae;

aux3 = acos(cosb);
aux4 = aux3 * 180/pi
c3 = aux4 - 30;

ze apare în urma diferenței dintre l3 (distanța inițială de la sol la vârf) și z

(distanța de la sol la vârf după selectarea coordonatelor x, y, z).

ze = z - l3;
s = sqrt(ze^2 + d^2);
alfae = sin(ze/s)*(180/pi);

După cum se vede avem nevoie de ze pentru a calcula latura S.

alfae ne ajută să aflăm unghiul c3 final adăugând la unghiul triunghiului

imaginar format din laturile a, b, c unghiul alfae, unghi determinat datorită
poziției în spațiu a vârfului.
Codul integral:

clear all
%pozitia dorita
x = 19;
y = 10;
z = 39;

id=1;
loadlibrary('dynamixel','dynamixel.h');
libfunctions('dynamixel');
port = 3;
BaudRate = 1;
res = calllib('dynamixel','dxl_initialize',port,BaudRate);
if res == 1
disp('Succeed to open USB2Dynamixel!');

StatusError=calllib('dynamixel','dxl_get_result');

% Cinematica inversa
l1 = 14.5; % Motor1 - motor2;
l2 = 17; % Motor2 - varf;
l3 = 30.3; % Baza - motor1;

ze = z - l3; %
d = sqrt(x^2 + y^2);
s = sqrt(ze^2 + d^2);
alfae = sin(ze/s)*(180/pi);

a = l2;
b = s;
c = l1;

cosa =(b^2 + c^2 - a^2) / (2*b*c);

cosb = (a^2 + c^2 - b^2) / (2*a*c);

aux1 = acos(cosa);
aux2 = aux1 * 180/pi;

aux3 = acos(cosb);
aux4 = aux3 * 180/pi

c1 = sin(d/c)*180/pi;
c2 = aux2 + alfae;
c3 = aux4 - 30;

calllib('dynamixel','dxl_write_word',7,32,30);
calllib('dynamixel','dxl_write_word',7,30,290*1024/300);
pause(5);

calllib('dynamixel','dxl_write_word',7,32,30);
calllib('dynamixel','dxl_write_word',7,30,100*1024/300);
pause(5);

calllib('dynamixel','dxl_write_word',7,32,30);
calllib('dynamixel','dxl_write_word',7,30,190*1024/300);
pause(5);

calllib('dynamixel','dxl_write_word',1,32,25);
calllib('dynamixel','dxl_write_word',1,30,150*1024/300);
pause(0)

calllib('dynamixel','dxl_write_word',3,32,35);
calllib('dynamixel','dxl_write_word',3,30,230*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',0,32,30);
calllib('dynamixel','dxl_write_word',0,30,150*1024/300);
pause(0);
calllib('dynamixel','dxl_write_word',2,32,25);
calllib('dynamixel','dxl_write_word',2,30,150*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',4,32,35);
calllib('dynamixel','dxl_write_word',4,30,150*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',5,32,30);
calllib('dynamixel','dxl_write_word',5,30,150*1024/300);
pause(5);

if y>=0
if x>=0
calllib('dynamixel','dxl_write_word',0,32,30);
calllib('dynamixel','dxl_write_word',0,30,(150+c1)*1024/300);
pause(0);
else
calllib('dynamixel','dxl_write_word',0,32,30);
calllib('dynamixel','dxl_write_word',0,30,(150-c1)*1024/300);
pause(0);
end

calllib('dynamixel','dxl_write_word',3,32,30);
calllib('dynamixel','dxl_write_word',3,30,(230-c2)*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',5,32,30);
calllib('dynamixel','dxl_write_word',5,30,c3*1024/300);
pause(0);

else

if x>=0
calllib('dynamixel','dxl_write_word',0,32,30);
calllib('dynamixel','dxl_write_word',0,30,(150-c1)*1024/300);
pause(0);
else
calllib('dynamixel','dxl_write_word',0,32,30);
calllib('dynamixel','dxl_write_word',0,30,(150+c1)*1024/300);
pause(0);
end

calllib('dynamixel','dxl_write_word',3,32,30);
calllib('dynamixel','dxl_write_word',3,30,(300-150-c1)*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',5,32,30);
calllib('dynamixel','dxl_write_word',5,30,(300-150+c1)*1024/300);
pause(0);
end

calllib('dynamixel','dxl_write_word',7,32,30);
calllib('dynamixel','dxl_write_word',7,30,100*1024/300);
pause(5);

calllib('dynamixel','dxl_write_word',7,32,30);
calllib('dynamixel','dxl_write_word',7,30,290*1024/300);
pause(5);

%
% calllib('dynamixel','dxl_write_word',3,32,45);
% calllib('dynamixel','dxl_write_word',3,30,299*1024/300);
% pause(5);
%
%
%
% calllib('dynamixel','dxl_write_word',7,32,30);
% calllib('dynamixel','dxl_write_word',7,30,200*1024/300);
% pause(2);
%
% calllib('dynamixel','dxl_write_word',3,32,45);
% calllib('dynamixel','dxl_write_word',3,30,200*1024/300);
% pause(2);

% POZITIE BLEG

pause(5); %Timp pina la blegire

calllib('dynamixel','dxl_write_word',0,32,30);
calllib('dynamixel','dxl_write_word',0,30,159*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',1,32,25);
calllib('dynamixel','dxl_write_word',1,30,123*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',2,32,25);
calllib('dynamixel','dxl_write_word',2,30,155*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',3,32,35);
calllib('dynamixel','dxl_write_word',3,30,283*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',4,32,35);
calllib('dynamixel','dxl_write_word',4,30,67*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',5,32,30);
calllib('dynamixel','dxl_write_word',5,30,154*1024/300);
pause(0);

calllib('dynamixel','dxl_write_word',7,32,30);
calllib('dynamixel','dxl_write_word',7,30,20*1024/300);
pause(8);

%Cuplu = 0 pentru toate motoarele

calllib('dynamixel','dxl_write_word',0,24,0);
calllib('dynamixel','dxl_write_word',1,24,0);
calllib('dynamixel','dxl_write_word',2,24,0);
calllib('dynamixel','dxl_write_word',3,24,0);
calllib('dynamixel','dxl_write_word',4,24,0);
calllib('dynamixel','dxl_write_word',5,24,0);
calllib('dynamixel','dxl_write_word',6,24,0);
calllib('dynamixel','dxl_write_word',7,24,0);

calllib('dynamixel','dxl_terminate');
disp('Succeed to close USB2Dynamixel!');
else
disp('Failed to open USB2Dynamixel!');
end
unloadlibrary('dynamixel');