kontro.m

三臂机器人仿真程序

function [t]=kontro(w1,l)

%Dieses Programm berechnet die y-Koordinaten der
%Gelenkpunkte und legt sie im Vektor Y ab. Ist eine
%Koordinate negativ, so wird der Kontrollparameter
%t=0 gesetzt, sonst wird t=1 uebergeben.
               
 Y(1,1)=0;
 Y(1,2)=l(1,1)*sin(w1(1,1));
 Y(1,3)=Y(1,2)-l(2,1)*sin(w1(1,1)+w1(2,1));
 Y(1,4)=Y(1,3)+l(3,1)*sin(w1(1,1)+w1(2,1)+w1(3,1));
 
 if Y>=0
   t=1;
 else
   t=0;
 end         %if-Struktur