clc;
clear;
close?all;
l?=?25;?%車長?-?cm
w?=?20;?%車寬?-?cm
v?=?5;?%車速?-?cm
Ts?=0.4;?%采樣時間間隔?-?second
N=2000;
fuzzyfis=readfis（‘car_parking.fis‘）;
%第一組試驗值
x（1）?=?10;?%汽車起點的x坐標
y（1）?=?30;?%汽車起點的y坐標
phi（1）?=-30;?%汽車車身起始傾角
%第二組試驗值
%x?=?20;?%汽車起點的x坐標
%y?=?30;?%汽車起點的y坐標
%phi?=?0;?%汽車車身起始傾角
%第三組試驗值
%x?=?20;?%汽車起點的x坐標
%y?=?30;?%汽車起點的y坐標
%phi?=?30;?%汽車車身起始傾角
%第四組試驗值
%x?=?20;?%汽車起點的x坐標
%y?=?30;?%汽車起點的y坐標
%phi?=?-30;?%汽車車身起始傾角
figure;
pause（1）;
for?i=1:1:N-1
pause（0.1）;
inputcanshu=[x（i）y（i）phi（i）];
outputcanshu=evalfis（inputcanshufuzzyfis）;
theta（i）=outputcanshu;
x（i+1）=x（i）+v*Ts*cos（phi（i）*pi/180）*cos（theta（i）*pi/180）;%計算新的x坐標
y（i+1）=y（i）+v*Ts*sin（phi（i）*pi/180）*cos（theta（i）*pi/180）;%計算新的y坐標
phi（i+1）=phi（i）-（v*Ts*300/l）*sin（theta（i）*pi/180）;%計算新的車身傾角
???if?x（i+1）?>=?100?||?y（i+1）?>=?90?%如果超出模糊邏輯控制器的輸入范圍，則停止
?????break;
???end
%fprintf（‘x?=?%f?y?=?%f?phi?=?%f?theta?=?%f?dx?=?%f?dy?=?%f\n‘?x?y
%theta?phi?delta_x?delta_y）;
x0?=?x（i）?+?w/2*sind（phi（i））;?%左前角x坐標
y0?=?y（i）?-?w/2*cosd（phi（i））;?%左前角y坐標
x1?=?x（i）?-?w/2*sind（phi（i））;?%右前角x坐標
y1?=?y（i）?+?w/2*cosd（phi（i））;?%右前角y坐標
p??=?x（i）?-?l*cosd（phi（i））;
q??=?y（i）?-?l*sind（phi（i））;
x2?=?p?+?w/2*sind（phi（i））;?%左后角x坐標
y2?=?q?-?w/2*cosd（phi（i））;?%左后角y坐標
x3?=?p?-?w/2*sind（phi（i））;?%右后角x坐標
y3?=?q?+?w/2*cosd（phi（i））;?%右后角y坐標
%畫汽車后軸中心
plot?（x（i）?y（i）?‘rs‘）;
axis（[-20?120?0?100]）;
xlabel（‘x?-?cm‘）;
ylabel（‘y?-?cm‘）;
title（‘Parallel?Parking?with?Model?Car‘）;
hold?on
grid?on;
%畫車位示意圖
h1?=?line（[-20?40]?[60?60]）;
h2?=?line（[40?40]?[60?95]）;
h3?=?line（[40?115]?[95?95]）;
h4?=?line（[115?115]?[60?95]）;
h5?=?line（[115?120]?[60?60]）;
set（h1?‘linewidth‘?5?‘color‘?‘b‘）;
set（h2?‘linewidth‘?5?‘color‘?‘b‘）;
set（h3?‘linewidth‘?5?‘color‘?‘b‘）;
set（h4?‘linewidth‘?5?‘color‘?‘b‘）;
set（h5?‘linewidth‘?5?‘color‘?‘b‘）;
%畫車身輪廓
l0?=?line（[x0?x1]?[y0?y1]）;
l1?=?line（[x1?x3]?[y1?y3]）;
l2?=?line（[x2?x3]?[y2?y3]）;
l3?=?line（[x0?x2]?[y0?y2]）;
set（l0?‘linewidth‘?4?‘color‘?‘m‘）;
set（l1?‘linewidth‘?2?‘color‘?‘b‘）;
set（l2?‘linewidth‘?4?‘color‘?‘g‘）;
set（l3?‘linewidth‘?2?‘color‘?‘b‘）;
end

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????1987??2013-12-03?20:34??car_parking.fis
?????文件????????2373??2020-03-13?10:31??car_parking.m