close all;
clc;
clear all;
K = MENU('chose','circle','square','Input set of points')
global a1 a2 a3 r incx incy x0 y0
np=48;%input('number of points to be evaluated=')
% 50,30,3,10,30,30,135 for ome=0
% 15,13.5,7.5,8,10,10,135 for ome=0
uiwait(msgbox('Input link lenghts a1,a2,a3','Have Ready the Inputs','warn'));
a1=15.5;%input('First link length a1=')
a2=13.5;%input(' Second link length a2=')
a3=6;%input('First link third length a3=')
%input('angle phi_h=')130
check= MENU('chose','Omega is not Zero','Omega is Zero')
t=0;
% determining the accuracy and convergence og phi_1 and phi_2
%=============================================================================
if (np<=20&np>0)%checking Continuity for Phi_1 and Phi_2
conv1=.7;
conv2=.7;
elseif (np<=60&np>20)
conv1=.4;
conv2=.4;
elseif (np<100&np>60)
conv1=.3;
conv2=.3;
end
%=============================================================================
if check==1
ome=input('Omega for the end effector position=')
phi_hFIX=135;
uiwait(msgbox('Omega is not Zero','Phi_h = Will Vary','warn'))
else
ome=0
check=2;
uiwait(msgbox('Omega is Zero','Phi_h=Constant','warn'));
phi_hFIX=135;%input('In Degrees phi_h='),135;
%320;%input('In Degrees phi_h='),30
end
t=0;
Tetha=0;
x1=10;
y1=5;
x2=20;
y2=5;
x3=20;
y3=15;
x4=10;
y4=15;
incx=(x3-x1)/(np/4);
incy=(y3-y1)/(np/4);
for i=1:np+1
switch K
case 1,
y0=10;%input('Center in Y of the circle='),30
x0=10;%input('Center in Y of the circle='),30
r=8;
ax=.2;
ay=.2;
[Vx,Vy,x_hc,y_hc,phi_hc]=circle(i,phi_hFIX,check,ome,t,Tetha);
Tetha=Tetha+360/np;
t=t+0.433;
%Calling function angles
x_h(i)=x_hc;
y_h(i)=y_hc;
phi_h(i)=phi_hc;
[phi_1f,phi_2f] = angles(i,phi_h(i),y_h(i),x_h(i));
phi_1(i,1)=phi_1f;
phi_2(i,1)=phi_2f;
case 2,
[x1,y1,x2,y2,x3,y3,x4,y4,ax,ay,Vx,Vy,x_hc,y_hc,phi_hc]=square(x1,y1,x2,y2,x3,y3,x4,y4,i,phi_hFIX,check,ome,t,Tetha,np);
Tetha=Tetha+360/np;
t=t+0.433;
x_h(i)=x_hc;
y_h(i)=y_hc;
phi_h(i)=phi_hc;
[phi_1f,phi_2f] = angles (i,phi_h(i),y_h(i),x_h(i));
phi_1(i,1)=phi_1f;
phi_2(i,1)=phi_2f;
case 3,
setpoints=load('setpoints.txt');
x_h=setpoints(:,1);
y_h=setpoints(:,2);
if (check==1)
phi_hc=ome*t*57.29;
else
phi_hc=phi_hFIX;
end
Tetha=Tetha+360/np;
t=t+0.433;
Vx=.5
Vy=.5
ax=.5
ay=.5
phi_h(i)=phi_hc
[phi_1f,phi_2f] =angles(i,phi_h(i),y_h(i),x_h(i))
phi_1(i,1)=phi_1f;
phi_2(i,1)=phi_2f;
end
%evaluating the inputs for a feasable solution
%To make posible the Velocity analysis,we should assign values a scalar to phi_1 and phi_2
syms X a11 a21 a31 ph_1 ph_2 ph_h ph_3 ;% Establishing all variables (new variables) as symbols
X=a11.*cos(ph_1)+a21.*cos(ph_1+ph_2)+a31.*cos(ph_1+ph_2+ph_3);
Y=a11.*sin(ph_1)+a21.*sin(ph_1+ph_2)+a31.*sin(ph_1+ph_2+ph_3);
%getting the Matrix "G"
%===================================================================================
Gx1=diff(X,ph_1);%Defifning G(1,:)
Gx2=diff(X,ph_2);
Gx3=diff(X,ph_3);
Gy1=diff(Y,ph_1);%Defifning G(2,:)
Gy2=diff(Y,ph_2);
Gy3=diff(Y,ph_3);
%===================================================================================
%getting the Matrix "h"
%===================================================================================
hx11=diff(Gx1,ph_1); %Defifning hx(1,:)
hx12=diff(Gx1,ph_2);
hx13=diff(Gx1,ph_3);
hx21=diff(Gx2,ph_1); %Defifning hx(2,:)
hx22=diff(Gx2,ph_2);
hx23=diff(Gx2,ph_3);
hx31=diff(Gx3,ph_1); %Defifning hx(3,:)
hx32=diff(Gx3,ph_2);
hx33=diff(Gx3,ph_3);
hy11=diff(Gy1,ph_1);%Defifning hy(1,:)
hy12=diff(Gy1,ph_2);
hy13=diff(Gy1,ph_3);
hy21=diff(Gy2,ph_1);%Defifning hy(2,:)
hy22=diff(Gy2,ph_2);
hy23=diff(Gy2,ph_3);
hy31=diff(Gy3,ph_1);%Defifning hy(3,:)
hy32=diff(Gy3,ph_2);
hy33=diff(Gy3,ph_3);
h11=1;
h12=1;
h13=1;
%===================================================================================
a11=a1;
a21=a2;
a31=a3;
ph_1=phi_1(i,1);
ph_2=phi_2(i,1);
ph_h=phi_h(i);
if check==1
ph_3=(ph_h)-phi_1(i,1)-phi_2(i);
else
ph_3=(phi_hFIX/57.29)-phi_1(i,1)-phi_2(i);
end
G=[Gx1 Gx2 Gx3;Gy1 Gy2 Gy3;1 1 1]; %Agrouping the derivatives in G
eval(G);
hx=[hx11 hx12 hx13;hx21 hx22 hx23;hx31 hx32 hx33];%Agrouping the derivatives in hx
hy=[hy11 hy12 hy13;hy21 hy22 hy23;hy31 hy32 hy33]; %Agrouping the derivatives in hy
h=[1 0 0;0 1 0;0 0 1];
eval(hx);
eval(hy);
V=[Vx;Vy;ome];
W=(eval(inv(G)*V));
a(:,i)=W(:,:);
W1(i,1)=a(1,i);
W2(i,1)=a(2,i);
W3(i,1)=a(3,i);
WW=[W1(i);W2(i);W3(i)]; %Setting the angular vel. in a vector WW=[W1;W2;W3]
%Building the f(phi,ome) same: Wt* h *W matrix
%=================================================================================
f1=WW'*hx*WW;
f2=WW'*hy*WW;
f3=WW'*h*WW;
f=[f1;f2;f3];
eval(f);
A=[ax;ay;.1];
alpha=eval(inv(G))*A-eval(f);
b(:,i)=alpha(:,:);
alpha1(i,1)=b(1,i);
alpha2(i,1)=b(2,i);
alpha3(i,1)=b(3,i);
%===================================================================================
if i==1
sw(1)=1;
elseif ((abs(phi_1(i)-phi_1(i-1))/abs(phi_1(i))<conv1)&((imag(phi_1(i)))==0)&(abs(phi_2(i)-phi_2(i-1))/abs(phi_1(i))<conv2)&((imag(phi_1(i)))==0))
sw(i)=1;
else sw(i)=0;
end
end
%=======================================================================================
if all(sw)==1
fid=fopen('Dimensions.txt','a');
fprintf (fid,'a1=%f\t a2=%f\t a3=%f\t phi_h=%f\n',a1,a2,a3,phi_h);
(msgbox('Succesfully Completed ,Solution Found','Success','warn'))
else
errordlg('Solution not Found,Try other inputs, Imaginary numbers in Solution','Fail','warn')
end
phi_1;
phi_2;
figure
for w=1:np+1
hold on;
g=[0;a1.*cos(phi_1(w))]; % Drawing First Arm
h=[0;a1.*sin(phi_1(w))];
line(g,h,'lineWidth',.5,'Color','r');
g=[a1.*cos(phi_1(w));a2.*cos(phi_2(w))+a1.*cos(phi_1(w))]; % Drawing Second Arm
h=[a1.*sin(phi_1(w));a2.*sin(phi_2(w))+a1.*sin(phi_1(w))];
line(g,h,'lineWidth',.5,'Color','g');
g=[a2.*cos(phi_2(w))+a1.*cos(phi_1(w));a3.*cos(phi_h(w)/57.29)+a2.*cos(phi_2(w))+a1.*cos(phi_1(w))]; % Drawing Third Arm
h=[a2.*sin(phi_2(w))+a1.*sin(phi_1(w));a3.*sin(phi_h(w)/57.29)+a2.*sin(phi_2(w))+a1.*sin(phi_1(w))];
line(g,h,'lineWidth',.5,'Color','b');
px=a3.*cos(phi_h(w)/57.29)+a2.*cos(phi_2(w))+a1.*cos(phi_1(w));
py=a3.*sin(phi_h(w)/57.29)+a2.*sin(phi_2(w))+a1.*sin(phi_1(w));
plot(px,py,'ro'); %Drawing the circle path
pause(0.01);%make a pause before the next figure
end
title('Figure 1: Robot Motion')
grid on;
hold off;
pause(2)
figure
w=(1:1:np+1);
plot(w,phi_1,w,phi_2,'-');
title('Figure 2: Phi_1 and Phi_2 Behavior (In Radians)')
legend('phi_1','phi_2',1)
grid on;
x_h=x_h';
y_h=y_h';
pause(2)
figure
plot(w,W1,w,W2,w,W3);
title('Figure 3: W1 and W2 Behavior (In Radians/ Seg )')
legend('W1','W2','W3',1)
grid on;
pause(2)
figure
plot(w,alpha1,w,alpha2,w,alpha3);
title('Figure 4: alpha1 and alpha2 and alpha3 Behavior (In Radians/ Seg^2 )')
legend('ALPHA1','ALPHA2','ALPHA3',1)
grid on;
end
%fid=fopen('setpoints.xls','a')
%fprintf (fid,'%d\t%d\n',x_h(i),y_h(i));
%fid=fclose('all')
