clc  % Clear Command Window
clear  %Remove items from workspace, freeing up system memory
close all % Remove specified figure

ts=0.1; % sample time
t=0:ts:30; % vector time

% Parameters
a=0.45;
b=0.3;
c=0.25;
R=0.3;


% Wheel velocities
Wd1=0.1*ones(1,length(t));
Wd2=-0.5*ones(1,length(t));
Wd3=0.1*ones(1,length(t));
Wd4=-0.5*ones(1,length(t));


% Transformation matrix (linear velocities to wheels velocity)
T=(1/R)*[1 -1 -(a+b);...
     1  1  (a+b);...
     1  1  -(a+b);...
     1 -1  (a+b)];  
 
invT=pinv(T);

% initial center position of robot
x1(1)=0;
y1(1)=0;
phi(1)=0;

% initial point control of robot
hx(1)=x1(1)-c*cos(phi(1));
hy(1)=y1(1)-c*sin(phi(1));

for k=1:length(t)
    
    Wd=[Wd1(k);Wd2(k);Wd3(k);Wd4(k)];

    v=invT*Wd; %Calculate linear and angular velocities

    uf(k)=v(1); %frontal linear velocity
    ul(k)=v(2); %lateral linear velocity
    w(k)=v(3); % angular velocity

   
    % Kinematic Model
    hxp(k)=uf(k)*cos(phi(k))-ul(k)*sin(phi(k))+c*w(k)*sin(phi(k));
    hyp(k)=uf(k)*sin(phi(k))+ul(k)*cos(phi(k))-c*w(k)*cos(phi(k));
    phip(k)=w(k);
    
    %numerical integral (Euler Method)
    hx(k+1)=hx(k)+ts*hxp(k)
    hy(k+1)=hy(k)+ts*hyp(k);
    phi(k+1)=phi(k)+ts*w(k);
    
    %center position of robot
    x1(k+1)=hx(k+1)+c*cos(phi(k+1));
    y1(k+1)=hy(k+1)+c*sin(phi(k+1));
    
    
end

scene=figure;  % new figure
tam=get(0,'ScreenSize');
set(scene,'position',[tam(1) tam(2) tam(3) tam(4)]); % position and size figure in the screen
axis equal; % Set axis aspect ratios
axis([-2 2 -2 2 -0.2 1]); % Set axis limits 
grid on; % Display axes grid lines

Dimension_Omni; % Parameters of robot
M1=Plot_Omni(x1(1),y1(1),phi(1)); % Plot robot in initial position hx,hy and phi orientation
hold on; % Retain current plot when adding new plot
M2=plot(hx(1),hy(2),'r'); % plot trayectory.
xlabel('x(m)'); ylabel('y(m)'); zlabel('z(m)'); % Label axis
camlight('rigth');
step=10; % position step

for i=1:step:length(t) % Loop emulation
    
    delete (M1)
     delete (M2)
    M1=Plot_Omni(x1(i),y1(i),phi(i)); hold on
        plot(hx(1:i),hy(1:i),'b','LineWidth',2); % plot trayectory.
    pause(ts)
end



