zielanfahren.m

三臂机器人仿真程序

function [w]=zielanfahren(p,l,w)

%Dieses Programm steuert die Berechnung der Winkelfuer die Ziellage,
%und uebernimmt die Bewegungsdarstellung, falls der Roboter diese
%ansteuern kann.

   w_alt=w;                     %Ablegen der alten Winkelwerte in w_alt
   w1=[pi/2; pi/2; pi/2];       %Startwert fuer Newton-Verfahren
   [w1,t]=newton(w1,p,l);       %Newton-Verfahren

   if t==1                      %Falls die Berechnung fuer die neuen
     w1=bereich(w1);            %Winkelwerte moeglich war, werden diese  
   end                          %in die gewuenschten Bereiche gesetzt.

   if t==1                      %Falls die Berechnung fuer die neuen
     t=kontrolle(w1,l);         %Winkelwerte moeglich war, wird geprueft,
   end                          %ob y-koordinaten der Ziellage positiv.

%Ausgabe: Falls der Zielpunkt nicht erreicht werden kann, wenn also keine
%         Konvergenz eintritt, oder eine y-Koordinate negativ ausfaellt,
%         wird eine entsprechende Meldung ausgegeben.
%         Sonst (t=1) wird die Bewegung des Roboterarmes dargestellt.
   if t==0 
     disp('!!! DIESER ZIELPUNKT KANN NICHT ANGEFAHREN WERDEN !!!');
     w=w_alt;                   %Rueckgabe der alten Winkelwerte
   else
      xstr=num2str(p(1,1));
      ystr=num2str(p(2,1));
      delta=p(3,1);
      deltastr=num2str(delta);
      text=['!!! Der Roboter faehrt den Punkt ( ',xstr,' | ',ystr,...
	    ' ) unter dem Winkel ',deltastr,' Grad an !!!'];
      disp(text);
      s=bewegung(w,w1,l);       %Aufruf der Bewegungsdarstellung
   %Falls bei der Bewegung die y-Koordinate fuer ein Armgelenk negativ
   %ausfaellt (s=0), wird der Roboterarm in die vorige Lage gesetzt.
    if s==0
      disp('!!!   DIESER ZIELPUNKT KANN NICHT ANGEFAHREN WERDEN    !!!'); 
      disp('!!! Der Roboter wird in die vorige Lage zurueckgesetzt !!!');
      graph(w,l);               %Darstellung der vorigen Lage
      w=w_alt;                  %Rueckgabe der alten Winkelwerte
    else
      w=w1;                     %Rueckgabe der neuen Winkelwerte
    end                         %if-Struktur
   end                          %if-Struktur