Code covered by the BSD License

# SCARA Manipulator

### MOHAMMED ELAMASSIE (view profile)

Full Software package for the SCARA manipulator (path planning and trajectory)

Path_PLaning(OI,OF,TH4)
```function Path_PLaning(OI,OF,TH4) % function take as input initial and
% final position and orientation

a1=1;
a2=1;
d1=1;                             % definition of robot parameters
d2=.1;
d=.1;
L=.3;

Xi=OI(1,1);
Yi=OI(2,1);                       % definition of initial position
Zi=OI(3,1);

flag=0;                            % make signal flag to 0 value

Xf=OF(1,1);
Yf=OF(2,1);                        % definition of final position
Zf=OF(3,1);

if sqrt(((Xi)^2)+((Yi)^2))>2     % test of initial position in or out
% work space
fprintf('Out of work space')

flag=1;                          % if yes make signal flag=1
end

if sqrt(Xf^2+Yf^2)>2             % test of final position in or out
% work space
fprintf('Out of work space')
flag=1;                          % if yes make signal flag=1
end

if flag==0                    % if flag=0 in this case we are in work
% space

[T1 T2 T4 d3]=Scara_Inverse(Xi,Yi,Zi,d1,d2,a1,a2) %call function
%Scara_Inverse
% returning T1 T2 T4 d3
% for initial point

J=Jacobian(T1,T2)                 %call function jacobian to test
% singularity of initial point
J11=[J(1:3,1:3)]
S=det(J11)
if S==0
flag=1
fprintf('Singularity')
end

[T1f T2f T4 d3]=Scara_Inverse(Xf,Yf,Zf,d1,d2,a1,a2) %call function
%Scara_Inverse
% returning T1 T2 T4 d3
% for initial point

J=Jacobian(T1f,T2f)                %call function jacobian to test
% singularity of final point
J11=[J(1:3,1:3)]
S=det(J11)
if S==0
flag=1
fprintf('Singularity')
end
end

%
if flag==0

slope = (Yf-Yi)/(Xf-Xi);           % we make calculation of slope of
% second portion of path
inc=.01

for Z=0:inc:Zi                  % begin of first portion of path,
% make the value of inc=0.01
% in this portion we maintain the
% value of X and Y and the only
% variation is in Z axis

d3=d1-Z-d2;

SCARA_plot(T1,T2,T4,a1,a2,d1,d3,d2,L,d)

hold on
plot3([Xi Xi],[Yi  Yi],[0 Zi],'red','linewidth',2) %plotting first
% portion of path

hold on

plot3([Xi Xf],[Yi  Yf],[Zi Zi],'red','linewidth',2) %plotting second
% portion of path

hold on

plot3([Xf Xf],[Yf  Yf],[Zi Zf],'red','linewidth',2) %plotting third
% portion of path

hold off

pause(.01)
end
if Xi>Xf                      % if the desired position is < the
% initial position we decrement xi by
% -0.01
inc=-.01;
else
inc=.01;                   % if the desired position is > the
% initial position we increment xi by
% +0.01
end

for X=Xi:inc:Xf           % for any new value of X and Y we calculate
% [T1 T2 T4 d2] by call of function
%  Scara_Inverse

Y=slope*(X-Xf)+Yf;
[T1 T2 T4 d3]=Scara_Inverse(X,Y,Zi,d1,d2,a1,a2);

plot3([Xi Xi],[Yi  Yi],[0 Zi],'red','linewidth',2)  %plotting first
% portion of path
hold on
plot3([Xi Xf],[Yi  Yf],[Zi Zi],'red','linewidth',2)%plotting second
% portion of path
hold on
plot3([Xf Xf],[Yf  Yf],[Zi Zf],'red','linewidth',2) %plotting third
% portion of path
hold on

SCARA_plot(T1,T2,T4,a1,a2,d1,d3,d2,L,d)     % ploting the motion of
% robot on the path

pause(.01)

end
inc=-.01;                   % begin of third portion of path,
% make the value of inc=-0.01
% in this portion we maintain the
% value of X and Y and the only
% variation is in Z axis

for Z=Zi:inc:Zf
d3=d1-Z-d2;                 % we go down by decrementing Zi

SCARA_plot(T1,T2,T4,a1,a2,d1,d3,d2,L,d)     % ploting the motion of
% robot on the path

hold on
plot3([Xi Xi],[Yi  Yi],[0 Zi],'red','linewidth',2) %plotting first
% portion of path

hold on

plot3([Xi Xf],[Yi  Yf],[Zi Zi],'red','linewidth',2) %plotting second
% portion of path

hold on

plot3([Xf Xf],[Yf  Yf],[Zi Zf],'red','linewidth',2)  %plotting third
% portion of path

hold off
pause(.01)
end

for T4=0:1:TH4            % adjustement and orientation of end
% effector
hold on
plot3([Xi Xi],[Yi  Yi],[0 Zi],'red','linewidth',2) %plotting first
% portion of path

plot3([Xi Xf],[Yi  Yf],[Zi Zi],'red','linewidth',2) %plotting second
% portion of path

plot3([Xf Xf],[Yf  Yf],[Zi Zf],'red','linewidth',2) %plotting third
% portion of path

hold off
SCARA_plot(T1,T2,T4,a1,a2,d1,d3,d2,L,d)     % ploting the motion of
% robot on the path
hold on
plot3([Xi Xi],[Yi  Yi],[0 Zi],'red','linewidth',2) %plotting first
% portion of path

plot3([Xi Xf],[Yi  Yf],[Zi Zi],'red','linewidth',2) %plotting second
% portion of path

plot3([Xf Xf],[Yf  Yf],[Zi Zf],'red','linewidth',2) %plotting third
% portion of path

pause(.01)

end
hold off
end```