14 Appendix A

APPENDIX A 57

Final Year Project MCT4198/4299 2008/2009

APPENDIX A MAIN PROGRAM % Starting points, end points and knot points xd=input('put desired x value:'); yd=input('put desired y value:'); %Check the distance rd=sqrt(xd^2+yd^2); if rd>=0.7; fprintf('Desired Position beyond the limit') break end n = 10; %No of segments m = 5; % total time l1=0.10; l2=0.10; %SUBROBOT_1 initial configuration xb1=0; yb1=0; x01 = 0.1987; %initial position of first subrobot EF,thus known y01 =-0.01712; x02=0.3977; y02=-0.03562; aligni=1; %if elbow up +1 and elbow down -1 [q1r1i,q2r1i,qr1i,q1d1i,q2d1i,qd1i]=inv_2link(x01,y01,l1,l2,aligni); %calculating initial orientation xb2=x01; %initial position of second subrobot base,thus known yb2=y01; x02i = x02-x01; %initial position of second subrobot EF,thus known y02i = y02-y01; [q1r2i,q2r2i,qr2i,q1d2i,q2d2i,qd2i]=inv_2link(x02i,y02i,l1,l2,aligni); %calculating initial orientation q1r2i=q1r2i-qr1i; x_elbow1i=l1*cos(q1r1i); xi1=x_elbow1i+l2*cos(q1r1i+q2r1i); y_elbow1i=l1*sin(q1r1i); yi1=y_elbow1i+l2*sin(q1r1i+q2r1i); x_elbow2i=xi1+l1*cos(q1r1i+q2r1i+q1r2i); xi2=x_elbow2i+l2*cos(q1r1i+q2r1i+q1r2i+q2r2i); y_elbow2i=yi1+l1*sin(q1r1i+q2r1i+q1r2i); yi2=y_elbow2i+l2*sin(q1r1i+q2r1i+q1r2i+q2r2i); x_i=[0,x_elbow1i,xi1,x_elbow2i,xi2]; y_i=[0,y_elbow1i,yi1,y_elbow2i,yi2]; %*******End OF INVERSE KINEMATIC SOLUTION OF INITIAL POSITION************* % determining ref point l1d=0.2;

APPENDIX A 58


l2d=0.2; alignd=1; [q1dr1,q2dr1,qrd1,q1dd1,q2dd1,qdd1]=inv_2link(xd,yd,l1d,l2d,alignd); xd_ref=l1d*cos(q1dr1); % reference point for 1st subrobot to go yd_ref=l1d*sin(q1dr1); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% xf1=(0+xd_ref+xd)/3.0; yf1=(0+yd_ref+yd)/3.0; % Obtain the pts on a straigth line from start point to end point x1= linspace(x01,xf1,n); y1=((yf1-y01)/(xf1-x01))*(x1-x01)+y01; align1=1; %To be decided elbow up or elbow down configuration % calculating inverse kinematics [q1r1,q2r1,qr1,q1d1,q2d1,qd1]=inv_2link(x1,y1,l1,l2,align1); delqr1=qr1(n)-qr1i; % Compute the (x,y)coordinates [x_elbow1_1,y_elbow1_1,xp_1,yp_1]=cor_2link(xb1,yb1,l1,l2,q1r1,q2r1); %Second subrobot %new position of Second Subrobot EF due to movement of first subrobot x02n=l1*cos(q1r2i+delqr1)+l2*cos(q1r2i+q2r2i+delqr1); y02n=l1*sin(q1r2i+delqr1)+l2*sin(q1r2i+q2r2i+delqr1); xf2 = xd-xf1; %final position of first subrobot EF,thus to be determined yf2 = yd-yf1; % Obtain the pts on a straigth line from start point to end point x2= linspace(x02n,xf2,n); y2=((yf2-y02n)/(xf2-x02n))*(x2-x02n)+y02n; align1=1; %To be decided elbow up or elbow down configuration % calculating inverse kinematics [q1r2,q2r2,qr2,q1d2,q2d2,qd2]=inv_2link(x2,y2,l1,l2,align1); % Compute the (x,y)coordinates xb2=xf1; yb2=yf1; [x_elbow1_2,y_elbow1_2,xp_2,yp_2]=cor_2link(xb2,yb2,l1,l2,q1r2,q2r2); [xdn,ydn]=geo_dn(x_elbow1_1(n),y_elbow1_1(n),x_elbow1_2(n),y_elbow1_2(n),xp_1(n),yp_1(n)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% dy1=yf1-y_elbow1_1(n); dx1=xf1-x_elbow1_1(n); ma=atan2(dy1,dx1); dyn=ydn-y_elbow1_1(n); dxn=xdn-x_elbow1_1(n); mn=atan2(dyn,dxn);

APPENDIX A 59


if ma> mn xf1=xdn; yf1=ydn; % Obtain the pts on a straigth line from start point to end point x1= linspace(x01,xf1,n); y1=((yf1-y01)/(xf1-x01))*(x1-x01)+y01; align1=1; %To be decided elbow up or elbow down configuration % calculating inverse kinematics [q1r1,q2r1,qr1,q1d1,q2d1,qd1]=inv_2link(x1,y1,l1,l2,align1); delqr1=qr1(n)-qr1i; % Compute the (x,y)coordinates xb1=0; yb1=0; [x_elbow1_1,y_elbow1_1,xp_1,yp_1]=cor_2link(xb1,yb1,l1,l2,q1r1,q2r1); %Second subrobot x02i = x02-x01; %initial position of first subrobot EF,thus known y02i = y02-y01; %new position of Second Subrobot EF due to movement of first subrobot x02n=l1*cos(q1r2i+delqr1)+l2*cos(q1r2i+q2r2i+delqr1); y02n=l1*sin(q1r2i+delqr1)+l2*sin(q1r2i+q2r2i+delqr1); xf2 = xd-xf1; %final position of first subrobot EF,thus to be determined yf2 = yd-yf1; % Obtain the pts on a straigth line from start point to end point x2= linspace(x02n,xf2,n); y2=((yf2-y02n)/(xf2-x02n))*(x2-x02n)+y02n; align1=1; %To be decided elbow up or elbow down configuration % calculating inverse kinematics [q1r2,q2r2,qr2,q1d2,q2d2,qd2]=inv_2link(x2,y2,l1,l2,align1); % Compute the (x,y)coordinates xb2=xf1; yb2=yf1; [x_elbow1_2,y_elbow1_2,xp_2,yp_2]=cor_2link(xb2,yb2,l1,l2,q1r2,q2r2); % plotting work space t=0:m/(n-1):m; theta=0:0.1:pi/2; wx_1=0.2*cos(theta); wy_1=0.2*sin(theta); wx_2=0.4*cos(theta); wy_2=0.4*sin(theta); plot(wx_1,wy_1,'-.g',wx_2,wy_2,'-.b','linewidth',2); axis normal xlabel('X (m)'), ylabel('Y(m)');

APPENDIX A 60


title('Robot configuration by using forward kinematics in virtual layers') hold on % plotting line trajectory plot(xp_1,yp_1,'r--','LineWidth',1); plot(xp_2,yp_2,'b--','LineWidth',1) % plotting robot configuration %plotting initial position plot(x_i,y_i,'r-o','LineWidth',2,'MarkerEdgeColor','r','MarkerFaceColor','b'); % lower robot configuration for i=1:1:n, x_1=[0,x_elbow1_1(i),xp_1(i)]; y_1=[0,y_elbow1_1(i),yp_1(i)]; plot(x_1,y_1,'b-*','LineWidth',2); end % upper robot configuration for i=1:1:n, x_2=[xb2,x_elbow1_2(i),xp_2(i)]; y_2=[yb2,y_elbow1_2(i),yp_2(i)]; plot(x_2,y_2,'b-*','LineWidth',2); end x_f=[0,x_elbow1_1(n),xp_1(n),x_elbow1_2(n),xp_2(n)]; y_f=[0,y_elbow1_1(n),yp_1(n),y_elbow1_2(n),yp_2(n)]; plot(x_f,y_f,'g-o','LineWidth',2,'MarkerEdgeColor','r','MarkerFaceColor','b'); hold off; q1r1f=q1r1;q2r1f=q2r1;q1r2f=q1r2-qr1;q2r2f=q2r2; figure plot(t,q1r1f,'r:*',t,q2r1f,'r:x',t,q1r2f,'r:v',t,q2r2f,'r:^','LineWidth',2); axis normal legend('\theta_1','\theta_2','\theta_3','\theta_4') xlabel('Time (sec)') ylabel('\theta_1,\theta_2,\theta_3,\theta_4'); title('Change of Joint variables') % plotting tracking error figure err_x=[x2+xp_1(n)]-[xp_2]; err_y=[y2+yp_1(n)]-[yp_2]; err_xy =sqrt((x2+xp_1(n)-xp_2).^2+(y2+yp_1(n)-yp_2).^2); plot(t,err_x,'b:+',t,err_y,'g:x',t,err_xy,'r:*','LineWidth',2); legend('err_x','err_y','err_xy') axis normal xlabel('Time (sec)'), ylabel('Error(m)'); title('tracking error of the EE') grid on

APPENDIX A 61


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% else %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % plotting work space t=0:m/(n-1):m; theta=0:0.1:pi/2; wx_1=0.2*cos(theta); wy_1=0.2*sin(theta); wx_2=0.4*cos(theta); wy_2=0.4*sin(theta); plot(wx_1,wy_1,'-.g',wx_2,wy_2,'-.b','linewidth',2); axis normal xlabel('X (m)'), ylabel('Y(m)'); title('Robot configuration by using forward kinematics in virtual layers') hold on % plotting line trajectory plot(xp_1,yp_1,'r--','LineWidth',1); plot(xp_2,yp_2,'b--','LineWidth',1) % plotting robot configuration %plotting initial position plot(x_i,y_i,'r-o','LineWidth',2,'MarkerEdgeColor','r','MarkerFaceColor','b'); % lower robot configuration for i=1:1:n, x_1=[0,x_elbow1_1(i),xp_1(i)]; y_1=[0,y_elbow1_1(i),yp_1(i)]; plot(x_1,y_1,'b-*','LineWidth',2); end % upper robot configuration for i=1:1:n, x_2=[xb2,x_elbow1_2(i),xp_2(i)]; y_2=[yb2,y_elbow1_2(i),yp_2(i)]; plot(x_2,y_2,'b-*','LineWidth',2); end x_f=[0,x_elbow1_1(n),xp_1(n),x_elbow1_2(n),xp_2(n)]; y_f=[0,y_elbow1_1(n),yp_1(n),y_elbow1_2(n),yp_2(n)]; plot(x_f,y_f,'g-o','LineWidth',2,'MarkerEdgeColor','r','MarkerFaceColor','b'); hold off; q1r1f=q1r1;q2r1f=q2r1;q1r2f=q1r2-qr1;q2r2f=q2r2; figure plot(t,q1r1f,'r:*',t,q2r1f,'r:x',t,q1r2f,'r:v',t,q2r2f,'r:^','LineWidth',2); axis normal legend('\theta_1','\theta_2','\theta_3','\theta_4') xlabel('Time (sec)') ylabel('\theta_1,\theta_2,\theta_3,\theta_4'); title('Change of Joint variables')

APPENDIX A 62


% plotting tracking error figure err_x=[x2+xp_1(n)]-[xp_2]; err_y=[y2+yp_1(n)]-[yp_2]; err_xy =sqrt((x2+xp_1(n)-xp_2).^2+(y2+yp_1(n)-yp_2).^2); plot(t,err_x,'b:+',t,err_y,'g:x',t,err_xy,'r:*','LineWidth',2); legend('err_x','err_y','err_xy') axis normal xlabel('Time (sec)'), ylabel('Error(m)'); title('tracking error of the EE') grid on end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %***************SOLUTION OF JOINT VARIABLES***************************** x1for=l1*cos(q1r1f); x2for=x1for+l2*cos(q1r1f+q2r1f); x3for=x2for+l1*cos(q1r1f+q2r1f+q1r2f); x4for=x3for+l2*cos(q1r1f+q2r1f+q1r2f+q2r2f); y1for=l1*sin(q1r1f); y2for=y1for+l2*sin(q1r1f+q2r1f); y3for=y2for+l1*sin(q1r1f+q2r1f+q1r2f); y4for=y3for+l2*sin(q1r1f+q2r1f+q1r2f+q2r2f); xfor=[0,x1for,x2for,x3for,x4for]; yfor=[0,y1for,y2for,y3for,y4for]; % plotting work space figure t=0:m/(n-1):m; theta=0:0.1:pi/2; wx_1=0.2*cos(theta); wy_1=0.2*sin(theta); wx_2=0.4*cos(theta); wy_2=0.4*sin(theta); plot(wx_1,wy_1,'-.r',wx_2,wy_2,'-.b','linewidth',2); axis normal xlabel('X (m)'), ylabel('Y(m)'); title('Robot configuration by using forward kinematics') hold on %plotting initial configuration x_i=[0,x_elbow1i,xi1,x_elbow2i,xi2]; y_i=[0,y_elbow1i,yi1,y_elbow2i,yi2]; plot(x_i,y_i,'r-o','LineWidth',2); for i=1:1:n, xfor=[0,x1for(i),x2for(i),x3for(i),x4for(i)]; yfor=[0,y1for(i),y2for(i),y3for(i),y4for(i)]; plot(xfor,yfor,'b-*','LineWidth',1); end %ploting final configuration xfor_f=[0,x1for(n),x2for(n),x3for(n),x4for(n)];

APPENDIX A 63


yfor_f=[0,y1for(n),y2for(n),y3for(n),y4for(n)]; plot(xfor_f,yfor_f,'g-*','LineWidth',2); % plotting tracking error figure x_err=[x4for]-[xp_2]; y_err=[y4for]-[yp_2]; res_err =sqrt((x4for-xp_2).^2+(y4for-yp_2).^2); plot(t,x_err,'b:+',t,y_err,'g:x',t,res_err,'r:*','LineWidth',2) axis normal xlabel('Time (sec)'), ylabel('Error(m)'); title('X,Y,& resultant error'); legend('err_x','err_y','err_xy') grid on %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% a1=q1r1f(n)*180*731/(90*pi); if a1

APPENDIX A 64


end if d2 < 90 k2=0 zd2=(127*d2)/90 else k2=1 nd2= 180-d2; zd2=(127*nd2)/90 end if d3 < 90 k3=0 zd3=(127*d3)/90 else k3=1 nd3= 180-d3; zd3=(127*nd3)/90 end if d4 < 90 k4=0 zd4=(127*d4)/90 else k4=1 nd4= 180-d4; zd4=(127*nd4)/90 end s = serial('COM7','BaudRate',9600); % OPEN SERIAL PORT %%%%%%%%%%%%%%SERVO 1%%%%%%%%%%%%%%%%% fopen(s); fwrite(s,128); %start byte fwrite(s,1); %device id fwrite(s,1); %command1 fwrite(s,0); %servo number 0-7

APPENDIX A 65


fwrite(s,128); %start byte fwrite(s,1); %device id fwrite(s,1); %command1 fwrite(s,1); %servo number 0-7

APPENDIX A 66


SUB PROGRAMS inv_2link.m file % calculating inverse kinematics function[q1r,q2r,qr,q1d,q2d,qd]=inv_2link(x,y,l1,l2,align) cq2 = (x.^2+ y.^2-l1.^2-l2.^2)/(2*l1*l2); if max(abs(cq2))>=1 fprintf('NOT Feasible LOCATION') return end % using Elbow-down configuration sq2 = align*sqrt(1-cq2.^2); q2r = atan2(sq2,cq2); k1 =(l1+l2.*cq2); k2 =l2.*sq2; q1r = atan2(y.*k1-(x.*k2), x.*k1+y.*k2); qr=q1r+q2r; q1d = 180*q1r/pi; q2d = 180*q2r/pi; qd=q1d+q2d;

geo_dn.m file % Compute the (x,y)coordinates of the joints function[xdn,ydn]=geo_dn(x1,y1,x2,y2,x3,y3); m12=(y2-y1)/(x2-x1); m34=-1/m12; c12=y1-m12*x1; c34=y3-m34*x3; xc=(c12-c34)/(m34-m12); yc=c12+m12*xc; xdn=2*xc-x3; ydn=2*yc-y3;

cor_2link.m file % Compute the (x,y)coordinates of the joints function[x_elbow1,y_elbow1,xp,yp]=cor_2link(xb,yb,l1,l2,q1r,q2r); % Compute the (x,y)coordinates of the elbows x_elbow1=xb+[l1.*cos(q1r)]; y_elbow1=yb+[l1.*sin(q1r)]; % Compute the (x,y)coordinates of the end-effector xp= x_elbow1+ l2.*cos(q1r+q2r); yp= y_elbow1+ l2.*sin(q1r+q2r);

14 Appendix A

Documents

Transcript of 14 Appendix A