clc;
clear all;
%initial position
l=0.45;
v(1)=-5;
x(1)=-15;
y(1)=5;
phi(1)=0;
theta(1)=pi/2;
% xe(1)=0;
% thetae(1)=0;
t=0.1;
angle1=readfis('angle'); %angle1=phi
vel=readfis('angle2');
correct=readfis('correct');
for i=1:300
xe(i)=x(i)-0; %車子距離x軸之誤差量
thetae(i)=pi/2-theta(i); %車子theta角與中心線的角度差

if y(i)<5 && abs(xe(i))<3 && abs(thetae(i))>pi/12 % 若滿足條件則將車子作前進修正動作
while abs(xe(i))>0.5 && abs(thetae(i))>pi/36 % 直到滿足許可之誤差量為止
phi(i 1)=evalfis([xe(i) thetae(i)],correct);
v(i)=5;

x(i 1)=v(i)*cos(theta(i))*cos(phi(i))*t x(i);
y(i 1)=v(i)*sin(theta(i))*cos(phi(i))*t y(i);
theta(i 1)=theta(i) v(i)*(sin(phi(i))/l)*t;

end
else
phi(i 1)=evalfis([theta(i) x(i)],angle1); % 倒車入庫控制
v(i 1)=evalfis([x(i) y(i)],vel);

x(i 1)=v(i)*cos(theta(i))*cos(phi(i))*t x(i);
y(i 1)=v(i)*sin(theta(i))*cos(phi(i))*t y(i);
theta(i 1)=theta(i) v(i)*(sin(phi(i))/l)*t;
end
end

以上程式是為了要做倒車入庫使用，而若無法順利倒車入庫時，則執行向前修正直到約趨於中心線為止，

再執行倒車完成任務。

PS. 請開 Untitled5.m 檔案

感謝大大!!

附加檔案：4a48e10e40f57_備份980520.rar

發生的問題是，他到while回圈裡面就無法跳出了