close all; clear all; clc; 
x=0; y=0; th=0; % initial state 
r=1; % r is robot radius

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Example of applying controls for state update of the 2D robot %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dT=0.05; 

t1=[0:dT:3.1]; %
u1=[0*ones(size(t1)); 1*ones(size(t1))];

t2=[0:dT:6];
u2=[1*ones(size(t2)); 0*ones(size(t2))];

t3=[0:dT:1.55];
u3=[0*ones(size(t3)); -1*ones(size(t3))];

t4=[0:dT:4.65];
u4=[6*ones(size(t4)); -1*ones(size(t4))];

t5=[0:dT:1.5];
u5=[0*ones(size(t5)); -1*ones(size(t5))];

t6=[0:dT:12];
u6=[atan(12-t6).*atan(t6)/atan(12).*(cos((t6.^5)/7200)); 0*ones(size(t6))];

t7=[0:dT:12];
u7=[0*ones(size(t7)); 0.8*cos(2*pi*3*t7/t7(1,size(t7,2)))];

t8=[0:dT:48];   % NOISY CONTROL
u8=[1*ones(size(t8))+0.9*randn(size(t8)); -1/3*ones(size(t8))+pi/2*(rand(size(t8))-0.5)];

u=[u1 u2 u3 u4 u5 u6 u7 u8]; % concatenate all controls into one

for i=1:size(u,2)
    if u(2,i)==0         % to avoid division by zero later
        u(2,i)=100*eps;  % "relatively small" number, but not "too small"
    end
end

for i=1:size(u,2)
    v=u(1,i); w=u(2,i);
    x=x-v/w*sin(th)+v/w*sin(th+w*dT);
    y=y+v/w*cos(th)-v/w*cos(th+w*dT);
    th=th+w*dT;
    plot(x,y,'ro','MarkerSize',24,'LineWidth',4); hold on;   % DRAWS ROBOTs CIRCLE
    plot([x; 1.2*r*cos(th)+x], [y; 1.2*r*sin(th)+y],'k-','LineWidth',2); % DRAWS BEARING
    axis(8*[-1 1 -1 1]); drawnow; hold off;
end