%%%%%%This?file?generate?smooth?polynomial?connected?Dstar?points?taking?the?points?near?the?obstacles?as?way-points%%%%%%%%%%%%%

%%%%%Author?and?Copyright:?Yi?Guo?2003%%%%%%%%%%%%

clear?all;
figure

l=0.8;rho=0.2;R=1;
t0=0;Ts=10;T=40;

Ts=[0?20?40];

path_points_dstar=[?10?5;9????6;8????6;?7????6;?6????7;?5????7;?4????8;3????9;3???10;...
????????3???11;3???12;?3???13;4???14;5???14;6???14;7???14;8???14;9???14;
????10???15;11???16;12???17;13???18;14???19;15???19;16???19;17???19;?...
????????18???19;19???19;20???19;21???19;22???19;23???19;24???19;25???19;26???19;27???19;
????28???19;29???19;30???19;31???19;32???19;33???19;34???19;35???19;36???19;37???18;...
????????38???18;39???18;40???18;41???18;42???18;43???18;44???18;
45???17;46???16;47???15;48???15;49???15;50???15;5015];

path_points=[path_points_dstar（1:）;path_points_dstar（8:）;path_points_dstar（12:）;...
????????path_points_dstar（25:）;path_points_dstar（42:）;path_points_dstar（60:）;path_points_dstar（60:）];

size_p=size（path_points）;
length_p=size_p（1）;

thetaf=0;
phif=0;

Length=0;

%%%%%%%%%%%%%%%%%%%For?loop%%%%%%%%%%%%%%%%%%%%%%%%%

for?j=1:length_p-2

x0=path_points（j1）;
y0=path_points（j2）;

xf=path_points（j+11）;
yf=path_points（j+12）;

%%%%%%%%%%%%%%%%If?xf~=x0
while?xf~=x0
????
theta0=thetaf;
phi0=phif;

thetaf=phif;
???
z10=x0-l*cos（theta0）/2;
z1f=xf-l*cos（thetaf）/2;

z20=tan（phi0）/（l*（cos（theta0）^3））;?%non-zero?phi
%z20=0;?%zero?phi
z30=tan（theta0）;
z40=y0-l*sin（theta0）/2;

a6min=0;
a6max=0;

z1=z10;
z2=z20;
z3=z30;
z4=z40;
z4_old=z40;

%?Initial?states

z1（1）=z10;
z2（1）=z20;?
z3（1）=z30;
z4（1）=z40;

x（1）=x0;
y（1）=y0;
theta（1）=theta0;
phi（1）=phi0;

z1f=xf-l/2*cos（thetaf）;?
z2f=1/l*tan（phif）/（cos（thetaf））^3;
z3f=tan（thetaf）;
z4f=yf-l/2*sin（thetaf）;

C=（z1f-z10）/40;
T1=0.01;
tt=0:T1:T;

xo1（1）=23;
yo1（1）=15;

r=0.5;
a61max=0;
a61min=0;

xo2（1）=50;
yo2（1）=20;
vo2x=[-0.15?0.15];?%0.2
vo2y=[-0.1?-0.29];?%-0.3

a62max=0;
a62min=0;

k1=1;

%if?j==4
%????k1=2;
%end
if?j==5
????k1=2;
end

for?k=1:k1????
????%?the?index?of?changing?the?initial?conditions
????index=Ts（k）/T1+1;
????????
????%?the?matrix?B?Y?A?in?the?boundary?conditions
????B=[?1?z1（index）?（z1（index））^2?（z1（index））^3?（z1（index））^4?（z1（index））^5;?...
????????0?1?2*z1（index）?3*（z1（index））^2?4*（z1（index））^3?5*（z1（index））^4;...
????????0?0?2?6*z1（index）?12*（z1（index））^2?20*（z1（index））^3;...
????????1?z1f?z1f^2?z1f^3?z1f^4?z1f^5;?...
????????0?1?2*z1f?3*z1f^2?4*z1f^3?5*z1f^4;...
????????0?0?2?6*z1f?12*z1f^2?20*z1f^3];
????
????Y=[z4（index）;?z3（index）;?z2（index）;?z4f;?z3f;?0];
????%Y=[z4（index）;?z3（index）;?z2（index）;?z4f;?z3f;?tan（phif）/（l*（cos（thetaf）^3））];

????A=[（z1（index））^6;?6*（z1（index））^5;?30*（z1（index））^4;?z1f^6;?6*z1f^5;?30*z1f^4];
???????
????a6（k）=0;
????
????if?j==4
????????vo1x=0.1;?%?[0?0.5?0.2];?%0.2
????????vo1y=0.2;?%