clc
clear all;
figure('DoubleBuffer','on')
X=0:0.01:10;
Y=sin(X);
plot(X,Y,'b')
hold on;
axis([0 10 -1.1 1.1]);
grid on;
x=0.1;y=0;X=0;Y=0;a0=0.785;a=0;jlc0=0;jlc1=0;
jdc0=0;jdc1=0;Kp=0;Ki=0;Kd=0;v=0;w=0;x0=0.1;
y0=0;v0=0;w0=0;
i=1;
while (X<10&i<1002)
X=x;
Y=sin(X);
jlc2=abs(Y-y);
jdc2=atan(sr(X))-a0;
Kp=2;
Ki=0.002;
Kd=0.006;
v0=Kp*(jlc2-jlc1)+Ki*jlc2+Kd*(jlc2-(2*jlc1)+jlc0);
w0=Kp*(jdc2-jdc1)+Ki*jdc2+Kd*(jdc2-(2*jdc1)+jdc0);
v=v+v0;
V(i)=v;
w=w+w0;
W(i)=w;
x=x0+v*cos(a0);
G(i)=x;
y=y0+v*sin(a0);
H(i)=y;
a=a0+w;
A(i)=a;
x0=x;
y0=y;
a0=a;
jlc0=jlc1;
jlc1=jlc2;
jdc0=jdc1;
jdc1=jdc2;
plot(x,y,'-.ro')
hold on;
axis([0 10 -1.1 1.1]);
pause(0.000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000001);
i=i+1;
end