clear all;

close all;
clc;
% Author = Saul Benitez
% NameFile = MobileRobotGraphics_V1

NUM_FILES = 7;
files = ['puntos_1.mat'; 'puntos_2.mat'; 'puntos_3.mat'; 'puntos_4.mat'; 'puntos_5.mat'; 'puntos_6.mat';
'puntos_6.mat'];

%Capture CAM data

for i = 1:NUM_FILES
cam_data(i, :, :) = load(files(i,:));
end

%Get length of vectors

lengthMaxFiles = zeros(1,2,1);
for i = 1:NUM_FILES
lengthFiles(:,:,i) = size(cam_data(i).ans);
if lengthFiles(1,1,i) > lengthMaxFiles(1,1,1)
lengthMaxFiles = lengthFiles(:,:,i);
end
end

%
coordinates = zeros(lengthMaxFiles(1), 3, NUM_FILES);
for i = 1:NUM_FILES
coordinates(1:lengthFiles(1,1,i),1,i) = cam_data(i).ans(:,6);
coordinates(1:lengthFiles(1,1,i),2,i) = cam_data(i).ans(:,8);
coordinates(1:lengthFiles(1,1,i),3,i) = cam_data(i).ans(:,4);
time(1:lengthFiles(1,1,i),1,i) = cam_data(i).ans(:,2);
offset(1,:,i) = coordinates(1,:,i);
end

%Get rotation angle

theta = coordinates(1,3,:)*pi/180;

%Delate offset of cameras

for i = 1:NUM_FILES
for j = 1:lengthFiles(1,1,i)
coordinates(j,:,i) = coordinates(j,:,i) - offset(1,:,i);
end
end

%Matrix rotation
for i = 1:NUM_FILES
R(:,:,i) = [cos(theta(:,:,i)), -sin(theta(:,:,i)), 0;
sin(theta(:,:,i)), cos(theta(:,:,i)), 0;
0, 0, 1];
end

%Rotation of results
for i = 1:NUM_FILES
for j = 1:lengthFiles(1,1,i)
coordinates(j,:,i) = (R(:,:,i) * coordinates(j,:,i)')';
end
end

MobileRobotCamGraphics_V1.txt[13/02/2017 21:16:15]
 %Plot data results
for i = 1:NUM_FILES
figure(i)
subplot(2,2,1)
plot(time(1:lengthFiles(1,1,i),1,i), coordinates(1:lengthFiles(1,1,i),1,i))
axis([0, 40, -0.55, 0.55]);
grid on
set(gca,'Xtick',0:8:32)
set(gca,'Ytick',-0.6:0.1:0.6)
xlabel('t(s)','FontSize', 14)
ylabel('X_w [m]','FontSize', 14)
SizeLeg = legend('$x_1$');
set(SizeLeg,'Interpreter','latex')
set(SizeLeg,'FontSize',20);

% figure(2)
subplot(2,2,2)
plot(time(1:lengthFiles(1,1,i),1,i), coordinates(1:lengthFiles(1,1,i),2,i))
axis([0, 40, -0.55, 0.55]);
grid on
set(gca,'Xtick',0:8:32)
set(gca,'Ytick',-0.6:0.1:0.6)
xlabel('t(s)','FontSize', 14)
ylabel('Y_w [m]','FontSize', 14)
SizeLeg = legend('$x_2$');
set(SizeLeg,'Interpreter','latex')
set(SizeLeg,'FontSize',20);

% figure(3)
subplot(2,2,3)
plot(time(1:lengthFiles(1,1,i),1,i), coordinates(1:lengthFiles(1,1,i),3,i))
axis([0, 40, -95, 95]);
grid on
set(gca,'Xtick',0:8:32)
set(gca,'Ytick',-90:45:180)
xlabel('t(s)','FontSize', 14)
ylabel('x_3 [rad]','FontSize', 14)
SizeLeg = legend('$x_3$');
set(SizeLeg,'Interpreter','latex')
set(SizeLeg,'FontSize',20);

% figure(4)
subplot(2,2,4)
plot(-coordinates(1:lengthFiles(1,1,i),1,i), coordinates(1:lengthFiles(1,1,i),2,i))
axis([-0.6, 0.6, -0.6, 0.6]);
grid on
set(gca,'Xtick',-0.6:0.1:0.6)
set(gca,'Ytick',-0.6:0.1:0.6)
xlabel('X_w [m]','FontSize', 14)
ylabel('Y_w [m]','FontSize', 14)
SizeLeg = legend('$(x_1,x_2)$');
set(SizeLeg,'Interpreter','latex')
set(SizeLeg,'FontSize',20);
end

MobileRobotCamGraphics_V1.txt[13/02/2017 21:16:15]