www.pudn.com > roboter.zip > zielanf.m

function [w]=zielanf(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=kontro(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