WMR mobile robot kinematics with sinusoidal wd

% WMR mobile robot kinematics with sinusoidal wd
clear; clf;
r = 0.05;L = 0.3;T= 0.1;TT= 5;N =TT/T;
x(1)=0; y(1)=0; phi(1)=pi/4;
vd = 0.5;
for k = 1:1:N
    % Inputs
    wd(k) = 0;
    %wd(k) = 2*pi*cos(2*pi*1*k*T);
    Wr(k)=1/r*vd+wd(k)*L/(2*r);
    Wl(k)=1/r*vd-wd(k)*L/(2*r);
    % Kinematics
    phi(k+1) = phi(k) + T*(Wr(k)-Wl(k))*r/L;
    x(k+1) = x(k) + T/2*(Wr(k)+Wl(k))*r*cos(phi(k));
    y(k+1) = y(k) + T/2*(Wr(k)+Wl(k))*r*sin(phi(k));
end
plot(x,y,'*')
axis('square')
title('mobile robot')
xlabel('x axis (m)')
ylabel('y axis (m)')
grid