Program for plotting the workspace of SCARA Robot.

q1 varies from -45 to +45

q2 varies from -20 to +20
q3 varies from 0mm to 200mm
d = [877 0 0 200]
a = [425 375 0 0]
Program:
clearvars;
i = 0;
d=[877 0 0 200]';
%joint distance
a=[425 375 0 0]';
%link lengths a1, a2, a3, a4
for q3=0:10:200
for q1=pi/4:(pi/90):7/4*pi
for q2=pi/9:pi/39:17/9*pi
i = i+1;
x(i) = a(1)*cos(q1)+a(2)*cos(q1-q2);
y(i) = a(1)*sin(q1)+a(2)*sin(q1-q2);
z(i) = d(1)-q3-d(4);
% all the possible points in the workspace
end
end
end
Z(1) = z(1);
n = 1;
for j=1:1:i
pubby = 0;
for k=1:1:n
if(Z(k) == z(j))
pubby = 1;
end
end
if (pubby ~= 1)
n = n + 1;
Z(n) = z(j);
end
end
Z = sort(Z);
npoints = 45;
% number of points in circumference of mesh
convergefactor = 1;
% convergence of selecting the point
ang1 = 2*pi/npoints;
ang2 = pi/npoints*convergefactor;
rajesh(n,npoints) = 0;
teta1(n,npoints) = 0;
teta2(n,npoints) = 0;
tetad(n,npoints) = ang2;
%Outer boundary points
for k = 1:1:i
radius = ((x(k)^2)+(y(k)^2))^(1/2);
tet = atan2(y(k),x(k));
if tet < 0
tet = 2*pi + tet;

end
for j = 1:1:n
if(z(k) == Z(j))
for raj = 1:1:npoints
tetc = ang1*(raj-1);
tetd = ((tet - tetc)^2)^0.5;
if (tetd > pi)
tetd = 2*pi - tetd;
end
tup = tetc + ang2;
twl = tetc;
tup2 = tetc;
twl2 = tetc - ang2;
if(0 < tetc) && (tetc < ang2)
tup2 = 2*pi;
twl2 = 2*pi+tetc-ang2;
twl = 0;
elseif(2*pi-ang2 < tetc) && (tetc < 2*pi)
tup2 = 2*pi;
twl2 = tetc - ang2;
tup = tup - 2*pi;
twl = 0;
end

if((((twl < tet) || (twl2 < tet)) && ...

((tet < tup) || (tet < tup2))) && ...
rajesh(j,raj) <= radius)
if (rajesh(j,raj) == radius)
if(tetad(j,raj)^2 > tetd^2)
teta1(j,raj) = tet;
teta2(j,raj) = tet;
tetad(j,raj) = tetd;
elseif (tetad(j,raj)^2 == tetd^2)
teta2(j,raj) = tet;
end
else
rajesh(j,raj) = radius;
teta1(j,raj) = tet;
teta2(j,raj) = tet;
tetad(j,raj) = tetd;
end
end
end
end
end
end

for j = 1:1:n
index = -1;
for raj = 1:1:npoints
index = index + 2;
X(j,index) = rajesh(j,raj) * cos(teta1(j,raj));

Y(j,index) = rajesh(j,raj) * sin(teta1(j,raj));

ZZ(j,index) = Z(j);
X(j,index+1) = rajesh(j,raj) * cos(teta2(j,raj));
Y(j,index+1) = rajesh(j,raj) * sin(teta2(j,raj));
ZZ(j,index+1) = Z(j);
end
end
X(:,index+2) = X(:,1);
Y(:,index+2) = Y(:,1);
ZZ(:,index+2) = ZZ(:,1);
%mesh(X,Y,ZZ);
%hold on;
%plot3(x,y,z,'.');
xxx = X;
yyy = Y;
zzz = ZZ;
% npoints = 36;
convergefactor = 0.3;
ang1 = 2*pi/npoints;
ang2 = pi/npoints*convergefactor;
rajesh(n,npoints) = max(rajesh(1,:));
teta1(n,npoints) = 0;
teta2(n,npoints) = 0;
tetad(n,npoints) = ang2;
%Inner boundary points

for k = 1:1:i
radius = ((x(k)^2)+(y(k)^2))^(1/2);
tet = atan2(y(k),x(k));
if tet < 0
tet = 2*pi + tet;
end
for j = 1:1:n
if(z(k) == Z(j))
for raj = 1:1:npoints
tetc = ang1*(raj-1);
tetd = ((tet - tetc)^2)^0.5;
if (tetd > pi)
tetd = 2*pi - tetd;
end
tup = tetc + ang2;
twl = tetc;
tup2 = tetc;
twl2 = tetc - ang2;
if(0 < tetc) && (tetc < ang2)
tup2 = 2*pi;
twl2 = 2*pi+tetc-ang2;
twl = 0;
elseif(2*pi-ang2 < tetc) && (tetc < 2*pi)
tup2 = 2*pi;
twl2 = tetc - ang2;
tup = tup - 2*pi;
twl = 0;
end

if((((twl < tet) || (twl2 < tet)) && ...

((tet < tup) || (tet < tup2))) && ...
rajesh(j,raj) >= radius)
if (rajesh(j,raj) == radius)
if(tetad(j,raj)^2 > tetd^2)
teta1(j,raj) = tet;
teta2(j,raj) = tet;
tetad(j,raj) = tetd;
elseif (tetad(j,raj)^2 == tetd^2)
teta2(j,raj) = tet;
end
else
rajesh(j,raj) = radius;
teta1(j,raj) = tet;
teta2(j,raj) = tet;
tetad(j,raj) = tetd;
end
end
end
end
end
end

for j = 1:1:n
index = -1;
for raj = 1:1:npoints
index = index + 2;
X(j,index) = rajesh(j,raj) *
Y(j,index) = rajesh(j,raj) *
ZZ(j,index) = Z(j);
X(j,index+1) = rajesh(j,raj)
Y(j,index+1) = rajesh(j,raj)
ZZ(j,index+1) = Z(j);
end
end
X(:,index+2) = X(:,1);
Y(:,index+2) = Y(:,1);
ZZ(:,index+2) = ZZ(:,1);

%merging outer and inner boundary

xxx = [xxx; flipdim(X,1)];
yyy = [yyy; flipdim(Y,1)];
zzz = [zzz; flipdim(ZZ,1)];
xxx(size(xxx,1)+1,:) = xxx(1,:);
yyy(size(yyy,1)+1,:) = yyy(1,:);
zzz(size(zzz,1)+1,:) = zzz(1,:);

%meshing the boundary

mesh(xxx,yyy,zzz);

cos(teta1(j,raj));
sin(teta1(j,raj));
* cos(teta2(j,raj));
* sin(teta2(j,raj));

Assignment 3
Workspace of SCARA Robot

Submitted by
J. Rajesh,
Sajan Kapil

Roll No.:114103100
Roll No.:11410394