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

function [w1,t]=iterati(w1,p,l) 
 
%Diese Programm beechnet den naechsten Iterationswert fuer 
%die Winkelwerte (Vektor w1). Ist diese Berechnung nicht moeglich, 
%d.h. die Funktionalmatrix ist nicht invertierbar, so wird der 
%Kontrollparameter t=0 zurueckgegeben, sonst t=1. 
 
%Aufstellen der Funktionalmatrix D 
 D=ones(3); 
 D(1,1)=-l(1,1)*sin(w1(1,1))+l(2,1)*sin(w1(1,1)+w1(2,1))-... 
         l(3,1)*sin(w1(1,1)+w1(2,1)+w1(3,1)); 
 D(1,2)= l(2,1)*sin(w1(1,1)+w1(2,1))-... 
         l(3,1)*sin(w1(1,1)+w1(2,1)+w1(3,1)); 
 D(1,3)=-l(3,1)*sin(w1(1,1)+w1(2,1)+w1(3,1)); 
 D(2,1)= l(1,1)*cos(w1(1,1))-l(2,1)*cos(w1(1,1)+w1(2,1))+... 
         l(3,1)*cos(w1(1,1)+w1(2,1)+w1(3,1)); 
 D(2,2)=-l(2,1)*cos(w1(1,1)+w1(2,1))+... 
         l(3,1)*cos(w1(1,1)+w1(2,1)+w1(3,1)); 
 D(2,3)= l(3,1)*cos(w1(1,1)+w1(2,1)+w1(3,1)); 
 
%Falls die Determinante zu D=0 ausfaellt (if), wird der 
%Parameter t=0 gesetzt, ansonsten wird mit der Berechnung 
%der naechsten Iterationswerte fuer die Winkel (Vektor w1) 
%fortgefahren. 
 if det(D)==0 
   t=0; 
 else 
   t=1; 
  %Berechnung der Inversen Matrix I zu D 
   E=eye(3,3); 
   I=D\E; 
  %Berechnung des Funktionswertes an der Stelle w1_n 
    f(1,1)=l(1,1)*cos(w1(1,1))-l(2,1)*cos(w1(1,1)+w1(2,1))+... 
           l(3,1)*cos(w1(1,1)+w1(2,1)+w1(3,1))-p(1,1); 
    f(2,1)=l(1,1)*sin(w1(1,1))-l(2,1)*sin(w1(1,1)+w1(2,1))+... 
           l(3,1)*sin(w1(1,1)+w1(2,1)+w1(3,1))-p(2,1); 
    f(3,1)=w1(1,1)+w1(2,1)+w1(3,1)-2*pi-(p(3,1)*pi)/180; 
  %Berechnung und Rueckgabe der neuen Winkelwerte w1_n+1 
    w1=w1-I*f; 
 end                %Ende der if-Struktur