graph.m

三臂机器人仿真程序

function [s]=graph(gepa,l)

%Berechnung der Koordinaten der 4 Gelenkpunkte des Roboters in Abhaenig-
%keit der Armlaengen (Vektor l) und der Gelenkparameter (Vektor gepa).
%x-Koordinaten werden im Reihenvektor x abgelegt,
%y-Koordinaten werden im Reihenvektor y abgelegt.
 X(1,1)=0;
 Y(1,1)=0;
 X(1,2)=l(1,1)*cos(gepa(1,1));
 Y(1,2)=l(1,1)*sin(gepa(1,1));
 X(1,3)=X(1,2)-l(2,1)*cos(gepa(1,1)+gepa(2,1));
 Y(1,3)=Y(1,2)-l(2,1)*sin(gepa(1,1)+gepa(2,1));
 X(1,4)=X(1,3)+l(3,1)*cos(gepa(1,1)+gepa(2,1)+gepa(3,1));
 Y(1,4)=Y(1,3)+l(3,1)*sin(gepa(1,1)+gepa(2,1)+gepa(3,1));

%Darstellung der Roboterlage, falls y-Koordinaten positiv
 if Y>=0 
   J=[2;3;4]; 
	 clf;
   plot(X,Y,X(J),Y(J),'bo','Linewidth',3,'MarkerFaceColor','r');
	 hold on; grid on;
   L=l(1,1)+l(2,1)+l(3,1);
   axis([-L, L, 0, L]);
   %axis('image')
   xlabel('x-Achse');
   ylabel('y-Achse');
   title('Roboterarm','FontSize',16,'Color','r');
   s=1; 
 else 
   s=0;  %Rueckgabe s=0, falls Darstellung nicht moeglich,
 end     %d.h. mindestens eine y-Koordinate negativ