دانشجویان کامپیوتر دانشگاه نبی اکرم

clear all
tx=input('Enter x:');
ty=input('Enter y:');
teta=input('Enter teta:');
fi=input('Enter fi:');
tt=teta;
teta=(teta*pi)/180;

axis equal
grid on
hold on
xlim([0 20]);
ylim([0 20]);

fs3=-40;
fs2=-25;
fs1=-10;
fce= 0;
fb1= 10;
fb2= 25;
fb3= 40;
%         -90 -10  40  90  140 190 270
mofi(1,:)=[fs3 fs2 fb1 fb3 fb3 fb3 fb3];
mofi(2,:)=[fs3 fs2 fs1 fb1 fb1 fb2 fb3];
mofi(3,:)=[fs3 fs3 fs2 fce fb2 fb3 fb3];
mofi(4,:)=[fs3 fs2 fs1 fs1 fb1 fb2 fb3];
mofi(5,:)=[fs3 fs3 fs3 fs3 fs1 fb2 fb3];

while abs(tx-10)>0.05 || abs(tt-90)>5 || ty<20

tx1=tx - sin(teta);
ty1=ty + cos(teta);

tx2=tx1 + 2*sin(teta);
ty2=ty1 - 2*cos(teta);

tx3=tx2 - 4*cos(teta);
ty3=ty2 - 4*sin(teta);

tx4=tx1 - 4*cos(teta);
ty4=ty1 - 4*sin(teta);

line([tx1 tx2 tx3 tx4 tx1],[ty1 ty2 ty3 ty4 ty1]);
plot(tx,ty,'r o')
pause(0.2)

fi=(fi*pi)/180;
% Left Wheel
l1tx=tx3 + cos(fi+teta)/2;
l1ty=ty3 + sin(fi+teta)/2;
l2tx=tx3 - cos(fi+teta)/2;
l2ty=ty3 - sin(fi+teta)/2;
line([l1tx l2tx],[l1ty l2ty]);
% Right Wheel
r1tx=tx4 + cos(fi+teta)/2;
r1ty=ty4 + sin(fi+teta)/2;
r2tx=tx4 - cos(fi+teta)/2;
r2ty=ty4 - sin(fi+teta)/2;
line([r1tx r2tx],[r1ty r2ty]);

x=tx;
l2= gaussmf(x,[1.5 0]);
l1= gaussmf(x,[0.7 6]);
cen=gaussmf(x,[0.2 10]);
r1= gaussmf(x,[0.7 14]);
r2= gaussmf(x,[1.5 20]);

mox=[l2 l1 cen r1 r2];

x=tt;
s3=gaussmf(x,[15 -90]);
s2=gaussmf(x,[10 -10]);
s1=gaussmf(x,[7 40]);
ce=gaussmf(x,[3.5 90]);
b1=gaussmf(x,[7 140]);
b2=gaussmf(x,[10 190]);
b3=gaussmf(x,[15 270]);

moteta=[s3 s2 s1 ce b1 b2 b3];

up=0;
down=0;
for i=1:5
    for j=1:7
        up = up + mox(i) * moteta(j) * mofi(i,j);
        down = down + mox(i) * moteta(j);
    end
end
if down~=0
 f=up/down;
else
    disp('DIVISION BY ZERO!!!');
end

tx=tx + cos(fi+teta) + sin(fi)*sin(teta);
ty=ty + sin(fi+teta) - sin(fi)*cos(teta);
teta= teta - asin(( 2*sin(fi) )/4);
tt=(teta*180)/pi;
fi=f;

end