TSR3

clear all;
clc;
close all;

kp=3; ka=8; kb=-1.5;
%xfinal=1;
%yfinal=14;
%x=5;y=5;
theta=pi/15;
T=0.03;
figure
%plot(x,y,'o'); hold on,grid on
% dx=yfinal-y;
 %dy=xfinal-x;
%
% p=sqrt(dx^2+dy^2);
% a=-theta+atan2(dy,dx);
% b=-theta-a;
%
% %legea de control
% v=kp*p;
% w=ka*a+kb*b;
%coordonate
%  purm = -kp*p*cos(a);
%  aurm =  kp*sin(a)-ka*a-kb*b;
%  burm = -kp*sin(a);
% xf=0;
% yf=0;
% zf=0;
% xsimi = R*cos(theta) + x;
% ysimi = R*cos(theta) + y;
%i=0;
h = findobj(gca,'Type','line')
%xxx=getpos
%yyy=getpos
    xlim([-15 15])
    ylim([-15 15])
    [xi,yi] = getpts
    xi';
    yi';

for i=1:7
     x= xi(i);
     y= yi(i);
    xfinal=xi(i+1);yfinal=yi(i+1);
    dy=yfinal-y;
    dx=xfinal-x;

       theta=pi/15;

while abs(dx)>0.1 && abs(dx)>0.1
   % plot(aurm,burm,'o'); hold on
    dy=yfinal-y;
    dx=xfinal-x;
    p=sqrt(dx^2+dy^2)
    a=-theta+atan2(dy,dx)
    b=-theta-a

    %
    v=kp*p
    w=ka*a+kb*b
    %
    x=x+T*v*cos(theta)
    y=y+T*v*sin(theta)
    theta=theta+T*w



    %Afisare
    xlim([-15 15])
    ylim([-15 15])
    plot(x,y,'or'); hold on
    line([x x+cos(theta)],[y y+sin(theta)])
    pause(0.05)

end

end