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

%%%%%%%%%%%%%%%%%%% Only position %%%%%%%%%%%%%%%%%%%
%%
% Position and orientation robot
hx=-1.5;
hy=0.5;
phi=pi/6;


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([-3 2 -2 2 -1 1]); % Set axis limits 
grid on; % Display axes grid lines


Dimension_Omni; % Parameters of robot
M1=Plot_Omni(hx,hy,phi); % Plot robot in position hx,hy and phi orientation
hold on; % Retain current plot when adding new plot
plot(hx,hy,'Or'); % plot circle in point control.
xlabel('x(m)'); ylabel('y(m)'); zlabel('z(m)'); % Label axis
camlight('rigth');
%%%%%%%%%%%%%%%%%%% vector position %%%%%%%%%%%%%%%%%%%
%%

ts=0.1; %sample time
tf=5; % final time
t=0:ts:tf; % vector time

% Position and orientation robot
hx=0*t;
hy=0.4*t;
phi=0*ones(1,length(t));

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([-3 2 -2 3 -0.1 1]); % Set axis limits 
grid on; % Display axes grid lines

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

step=2;

for i=1:step:length(t) % Loop emulation
    
    delete (M1)
    M1=Plot_Omni(hx(i),hy(i),phi(i)); hold on
    
    pause(ts)
end

