L0=0;L(1)=0;L(2)=3.72;L(3)=1.62;L(4)=0.968;x0=3.5;xf=2;z0=0;zf=0;m=0;%挖掘機(jī)參數(shù)
af=atan((z0-zf)./(x0-xf));%判斷所給直線的角度，或者直接給出直線的角度，則更加的方便！
%while 23*pi/180<=beita<=149.5*pi/180 | -51*pi/180<afa<75*pi/180
t=0:0.1:20;
x=x0-0.1*t.*cos(af);%速度為0.1m/s
z=z0-0.1*t.*sin(af);
y=0;
sita1=atan(y./x);
sita4=0;
gama=atan(z./x);
afa=acos((x.^2+z.^2+L(2)^2-(L(3)+L(4))^2)./(2*sqrt(x.^2+z.^2)*L(2)))+gama;%動(dòng)臂轉(zhuǎn)角
beita=acos(((L(2)^2+(L(3)+L(4))^2)-x.^2-z.^2)./(2*L(2)*(L(3)+L(4))));%斗桿轉(zhuǎn)角
%end
for i=1:1:length(beita)
if  23*pi/180<beita(i)&180<beita(i)<149.5*pi/180 & -51*pi/180<afa(i)&afa(i)<75*pi/180
    m=m+1;
end
end
for i1=1:1:m
    afa0(i1)=afa(i1);
    beita0(i1)=beita(i1);
end
b=L(2)*cos(afa0)+(L(3)+L(4))*cos(beita0+afa0-pi);
a=L(2)*sin(afa0)+(L(3)+L(4))*sin(beita0+afa0-pi);
plot(b,a)
title('挖掘機(jī)鏟斗軌跡曲線')
xlabel('時(shí)間（秒）')
ylabel('軌跡（米）')
grid
%鏟斗與斗桿的相對轉(zhuǎn)角為零！