1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
| function Q=MGI_SCARA(coord)
l1=200;
l2=200;
x=coord(1);
y=coord(2);
z=coord(3);
psy=coord(4);
% seul Q4 donne la valeur de Z donc Q4=Z
q4=-z;
% pour une position xy donnée deux coordonnées articulaires donne le bon
% resultat. on choisira l'une des deux.
beta=acos((l1^2+(sqrt(x^2+y^2))^2-l2^2)/(2*l1*sqrt(x^2+y^2)));
% methode incrementale:
for i1=-pi:0.01:pi
for i2=-pi:0.01:pi
xeval(round((pi+i1)*100+1),round((pi+i2)*100+1))=200*cos(i1)*cos(i2)-200*sin(i1)*sin(i2)+200*cos(i1);
yeval(round((pi+i1)*100+1),round((pi+i2)*100+1))=200*sin(i1)*cos(i2)+200*cos(i1)*sin(i2)+200*sin(i1);
end
end
Dx=abs(x-xeval);
Dy=abs(y-yeval);
m=Dx.*Dy;
[a,b]=find(m==min(min(m)));
q1=(a-1)/100-pi;
q2=(b-1)/100-pi;
q3=q1+q2-psy;
Q=[q1;q2;q3;q4];
end |
