Commit c4e17b93 authored by Farid Alijani's avatar Farid Alijani
Browse files

instruction to run the docking

parent b8c10bfc
......@@ -149,7 +149,7 @@ Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
figure;
set(gcf,'color','white');
subplot(3,1,1);
subplot(321);
plot(t_P_sec_data_1,Pose_X_data_1,'LineWidth',3);
hold on
plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
......@@ -163,7 +163,7 @@ l2 = legend('Left', 'Right');
set(l2,'interpreter','latex','FontSize',11);
% legend('boxoff');
subplot(3,1,2);
subplot(323);
plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',3);
hold on
plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
......@@ -171,7 +171,7 @@ plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
% plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6);
ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
subplot(3,1,3);
subplot(325);
plot(t_P_sec_data_1,Theta_data_1,'LineWidth',3);
hold on
plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
......@@ -181,11 +181,11 @@ plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
xlabel('$ time$ [s]','interpreter','latex','FontSize',14);
ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);
% control signals
figure;
set(gcf,'color','white');
% % control signals
% figure;
% set(gcf,'color','white');
subplot(311);
subplot(322);
plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',3);
hold on
......@@ -199,7 +199,7 @@ l2 = legend('${Left}$', '${Right}$');
set(l2,'interpreter','latex','FontSize',11);
legend('boxoff')
subplot(3,1,2);
subplot(324);
plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',3);
hold on
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
......@@ -208,7 +208,7 @@ plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
subplot(3,1,3);
subplot(326);
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',3);
hold on
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
......
......@@ -8,9 +8,10 @@ yaw_off = pi;
% data 1: 556 ros messages for geometry_msgs/Twist
% 557 ros messages for geometry_msgs/PoseStamped
%
% Pose_Matrix_data_1 = csvread('POSE_with_Py0.86_Iy0_Dy0.002ANDPt0.08_It0_Dt0.txt',1,0);
% Vel_Matrix_data_1 = csvread('VEL_with_Py0.86_Iy0_Dy0.002ANDPt0.08_It0_Dt0.txt',1,0);
Pose_Matrix_data_1 = csvread('CAM_PID_.28_.0001_.01.txt',1,0);
Vel_Matrix_data_1 = csvread('vel_PID_.28_.0001_.01.txt',1,0);
......@@ -40,7 +41,7 @@ t_V_sec_data_1(:,size(t_V_sec_data_1,2)) = [];
% theta-axis gains:
% P = .28; I = 0.1; D = 0.15;
%
% %
% Pose_Matrix_data_2 = csvread('POSE_with_Py0.66_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
% Vel_Matrix_data_2 = csvread('VEL_with_Py0.66_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
%
......@@ -73,7 +74,7 @@ t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = [];
% theta-axis gains:
% P = .08; I = 0; D = 0;
%
% Pose_Matrix_data_3 = csvread('POSE_with_Py0.86_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
% Vel_Matrix_data_3 = csvread('VEL_with_Py0.86_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
......@@ -164,22 +165,22 @@ Pose_Matrix_data_6 = csvread('CAM_PID_.39_.0005_.01.txt',1,0);
Vel_Matrix_data_6 = csvread('vel_PID_.39_.0005_.01.txt',1,0);
% time in position
TimeP_ros_data_6 = Pose_Matrix_data_6(:,1); % ros time, needs to be converted to sec...
TP_ros_6 = Pose_Matrix_data_6(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationP_data_6 = (TimeP_ros_data_6(size(TimeP_ros_data_6,1),:) - TimeP_ros_data_6(1,:))*10^(-9);
duP_6 = (TP_ros_6(size(TP_ros_6,1),:) - TP_ros_6(1,:))*10^(-9);
t_P_sec_data_6 = 0:durationP_data_6/size(TimeP_ros_data_6,1):durationP_data_6;
t_P_sec_data_6(:,size(t_P_sec_data_6,2)) = [];
t_P_ave_6 = 0:duP_6/size(TP_ros_6,1):duP_6;
t_P_ave_6(:,size(t_P_ave_6,2)) = [];
% time in vel
TimeV_ros_data_6 = Vel_Matrix_data_6(:,1); % ros time, needs to be converted to sec...
TV_ros_6 = Vel_Matrix_data_6(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationV_data_6 = (TimeV_ros_data_6(size(TimeV_ros_data_6,1),:) - TimeV_ros_data_6(1,:))*10^(-9);
duV_6 = (TV_ros_6(size(TV_ros_6,1),:) - TV_ros_6(1,:))*10^(-9);
t_V_sec_data_6 = 0:durationV_data_6/size(TimeV_ros_data_6,1):durationV_data_6;
t_V_sec_data_6(:,size(t_V_sec_data_6,2)) = [];
t_V_ave_6 = 0:duV_6/size(TV_ros_6,1):duV_6;
t_V_ave_6(:,size(t_V_ave_6,2)) = [];
%% Pose estimation
......@@ -282,14 +283,14 @@ ref_Pose_data_5 =[ref_X_data_5;ref_Y_data_5];
%data 6:
% when using marker pose ,,,,
Pose_X_data_6 = Pose_Matrix_data_6(:,4);
Pose_Y_data_6 = Pose_Matrix_data_6(:,3);
Theta_data_6 = Pose_Matrix_data_6(:,7);
X_6 = Pose_Matrix_data_6(:,4);
Y_6 = Pose_Matrix_data_6(:,3);
Theta_6 = Pose_Matrix_data_6(:,7);
% Extracting reference values when the robot is manually docked!
ref_X_data_6 = Pose_X_data_6(size(Pose_X_data_6,1));
ref_Y_data_6 = Pose_Y_data_6(size(Pose_Y_data_6,1));
ref_Theta_data_6 = Theta_data_6(size(Theta_data_6,1));
ref_X_data_6 = X_6(size(X_6,1));
ref_Y_data_6 = Y_6(size(Y_6,1));
ref_Theta_data_6 = Theta_6(size(Theta_6,1));
ref_Pose_data_6 =[ref_X_data_6;ref_Y_data_6];
% Reference Circle
......@@ -329,9 +330,9 @@ Vel_Y_data_5 = Vel_Matrix_data_5(:,3);
Omega_Z_data_5 = Vel_Matrix_data_5(:,7);
% data 6:
Vel_X_data_6 = Vel_Matrix_data_6(:,2);
Vel_Y_data_6 = Vel_Matrix_data_6(:,3);
Omega_Z_data_6 = Vel_Matrix_data_6(:,7);
X_dot_6 = Vel_Matrix_data_6(:,2);
Y_dot_6 = Vel_Matrix_data_6(:,3);
theta_dot_6 = Vel_Matrix_data_6(:,7);
%% Plots
......@@ -346,10 +347,10 @@ Omega_Z_data_6 = Vel_Matrix_data_6(:,7);
% xlabel('time');
% ylabel('\theta');
% camera pose estimation
figure;
set(gcf,'color','white');
subplot(3,1,1);
subplot(321);
plot(t_P_sec_data_1,Pose_X_data_1,'LineWidth',2.1);
hold on
plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
......@@ -360,11 +361,11 @@ plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.5);
title('Pose estimation in marker coordinate system');
ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14);
l2 = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$');
set(l2,'interpreter','latex','FontSize',11);
legend('boxoff');
% l2 = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$');
% set(l2,'interpreter','latex','FontSize',11);
% legend('boxoff');
subplot(3,1,2);
subplot(323);
plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',2.1);
hold on
plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
......@@ -374,7 +375,7 @@ hold on
plot(t_P_sec_data_4,Pose_Y_data_4,'m','LineWidth',1.5);
ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
subplot(3,1,3);
subplot(325);
plot(t_P_sec_data_1,Theta_data_1,'LineWidth',2.1);
hold on
plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
......@@ -388,9 +389,9 @@ ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);
% control signals
figure;
set(gcf,'color','white');
subplot(3,1,1);
% figure;
% set(gcf,'color','white');
subplot(322);
plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',2.1);
hold on
......@@ -408,7 +409,7 @@ set(l2,'interpreter','latex','FontSize',11);
legend('boxoff');
subplot(3,1,2);
subplot(324);
plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',2.1);
hold on
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
......@@ -418,7 +419,7 @@ hold on
plot(t_V_sec_data_4,Vel_Y_data_4,'m','LineWidth',1.5);
ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
subplot(3,1,3);
subplot(326);
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',2.1);
hold on
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
......@@ -460,8 +461,22 @@ ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
% quiver(Pose_Y_data_1,Pose_X_data_1,Vy,Vx,'r--','LineWidth',.1);
figure;
plot(Pose_Y_data_6,Pose_X_data_6,'k','LineWidth',1.4);
axis([-.3 .7 .1 1.6]);
plot(Y_6,X_6,'k','LineWidth',1.4);
% axis([-.3 .7 .1 1.6]);
figure;
subplot(321);
plot(t_P_ave_6,X_6,'LineWidth',2.1);
subplot(322);
plot(t_V_ave_6,X_dot_6,'LineWidth',2.1);
subplot(323);
plot(t_P_ave_6,Y_6,'LineWidth',2.1);
subplot(324);
plot(t_V_ave_6,Y_dot_6,'LineWidth',2.1);
subplot(325);
plot(t_P_ave_6,Theta_6,'LineWidth',2.1);
subplot(326);
plot(t_V_ave_6,theta_dot_6,'LineWidth',2.1);
figure;
......
clc;
close all;
clear all;
%% Import data and time conversion
% data 1: 556 ros messages for geometry_msgs/Twist
% 557 ros messages for geometry_msgs/PoseStamped
% y-axis gains:
% (010816_1718) best!!!
% P = .4; I = 0; D = 0;
% y-axis gains:
% (280716_2024) better!!!
% P = .44; I = 0; D = 0;
% theta-axis gains:
% P = .08; I = 0; D = 0;
% y-axis gains:
% (280716_2002)
% P = .46; I = 0; D = 0;
% theta-axis gains:
% P = .08; I = 0; D = 0;
% Pose_Matrix_data_1 = csvread('Pose01_08_16_1718.txt',1,0);
% Vel_Matrix_data_1 = csvread('Velocity01_08_16_1718.txt',1,0);
Pose_Matrix_data_1 = csvread('CAM_x_dot_.1.txt',1,0);
Vel_Matrix_data_1 = csvread('x_dot_.1.txt',1,0);
% time in position
TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationP_data_1 = (TimeP_ros_data_1(size(TimeP_ros_data_1,1),:) - TimeP_ros_data_1(1,:))*10^(-9);
t_P_sec_data_1 = 0:durationP_data_1/size(TimeP_ros_data_1,1):durationP_data_1;
t_P_sec_data_1(:,size(t_P_sec_data_1,2)) = [];
% time in vel
TimeV_ros_data_1 = Vel_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationV_data_1 = (TimeV_ros_data_1(size(TimeV_ros_data_1,1),:) - TimeV_ros_data_1(1,:))*10^(-9);
t_V_sec_data_1 = 0:durationV_data_1/size(TimeV_ros_data_1,1):durationV_data_1;
t_V_sec_data_1(:,size(t_V_sec_data_1,2)) = [];
% -----------------------------------------------------------------------------------------------------
% data 2:
% py = .25; ptheta = .25;
% iy = 0; itheta = 0;
% dy = 0; dtheta = 0;
% Pose_Matrix_data_2 = csvread('Pose01_08_16_1749.txt',1,0);
% Vel_Matrix_data_2 = csvread('Velocity01_08_16_1749.txt',1,0);
Pose_Matrix_data_2 = csvread('CAM_x_dot_.15.txt',1,0);
Vel_Matrix_data_2 = csvread('x_dot_.15.txt',1,0);
% time in position
TimeP_ros_data_2 = Pose_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationP_data_2 = (TimeP_ros_data_2(size(TimeP_ros_data_2,1),:) - TimeP_ros_data_2(1,:))*10^(-9);
t_P_sec_data_2 = 0:durationP_data_2/size(TimeP_ros_data_2,1):durationP_data_2;
t_P_sec_data_2(:,size(t_P_sec_data_2,2)) = [];
% time in vel
TimeV_ros_data_2 = Vel_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationV_data_2 = (TimeV_ros_data_2(size(TimeV_ros_data_2,1),:) - TimeV_ros_data_2(1,:))*10^(-9);
t_V_sec_data_2 = 0:durationV_data_2/size(TimeV_ros_data_2,1):durationV_data_2;
t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = [];
% -----------------------------------------------------------------------------------------------------
% data 3:
% Pose_Matrix_data_3 = csvread('Pose01_08_16_1831.txt',1,0);
% Vel_Matrix_data_3 = csvread('Velocity01_08_16_1831.txt',1,0);
Pose_Matrix_data_3 = csvread('CAM_x_dot_.16.txt',1,0);
Vel_Matrix_data_3 = csvread('x_dot_.16.txt',1,0);
% time in position
TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationP_data_3 = (TimeP_ros_data_3(size(TimeP_ros_data_3,1),:) - TimeP_ros_data_3(1,:))*10^(-9);
t_P_sec_data_3 = 0:durationP_data_3/size(TimeP_ros_data_3,1):durationP_data_3;
t_P_sec_data_3(:,size(t_P_sec_data_3,2)) = [];
% time in vel
TimeV_ros_data_3 = Vel_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
% duriation = end_time - start_time
durationV_data_3 = (TimeV_ros_data_3(size(TimeV_ros_data_3,1),:) - TimeV_ros_data_3(1,:))*10^(-9);
t_V_sec_data_3 = 0:durationV_data_3/size(TimeV_ros_data_3,1):durationV_data_3;
t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];
%% Pose estimation
%data 1:
% when using marker pose ,,,,
Pose_X_data_1 = Pose_Matrix_data_1(:,4);
Pose_Y_data_1 = Pose_Matrix_data_1(:,3);
Theta_data_1 = Pose_Matrix_data_1(:,7);
% Extracting reference values when the robot is manually docked!
ref_X_data_1 = Pose_X_data_1(size(Pose_X_data_1,1));
ref_Y_data_1 = Pose_Y_data_1(size(Pose_Y_data_1,1));
ref_Theta_data_1 = Theta_data_1(size(Theta_data_1,1));
ref_Pose_data_1 =[ref_X_data_1;ref_Y_data_1];
%data 2:
% when using marker pose ,,,,
Pose_X_data_2 = Pose_Matrix_data_2(:,4);
Pose_Y_data_2 = Pose_Matrix_data_2(:,3);
Theta_data_2 = Pose_Matrix_data_2(:,7);
% Extracting reference values when the robot is manually docked!
ref_X_data_2 = Pose_X_data_2(size(Pose_X_data_2,1));
ref_Y_data_2 = Pose_Y_data_2(size(Pose_Y_data_2,1));
ref_Theta_data_2 = Theta_data_2(size(Theta_data_2,1));
ref_Pose_data_2 =[ref_X_data_2;ref_Y_data_2];
%data 3:
% when using marker pose ,,,,
Pose_X_data_3 = Pose_Matrix_data_3(:,4);
Pose_Y_data_3 = Pose_Matrix_data_3(:,3);
Theta_data_3 = Pose_Matrix_data_3(:,7);
% Extracting reference values when the robot is manually docked!
ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1));
ref_Y_data_3 = Pose_Y_data_3(size(Pose_Y_data_3,1));
ref_Theta_data_3 = Theta_data_3(size(Theta_data_3,1));
ref_Pose_data_3 =[ref_X_data_3;ref_Y_data_3];
thresh_X = .001;
theta = 0:.001:2*pi;
% needs to be adjusted manually if docking platform is replaced!
ref_x = -.203;
ref_y = -.019;
x_circle = thresh_X*cos(theta) + ref_x; % ref_X needs to be recorded
y_circle = thresh_X*sin(theta) + ref_y; % ref_Y needs to be recorded
%% Velocity estimation
% data 1:
Vel_X_data_1 = Vel_Matrix_data_1(:,2);
Vel_Y_data_1 = Vel_Matrix_data_1(:,3);
Omega_Z_data_1 = Vel_Matrix_data_1(:,7);
% data 2:
Vel_X_data_2 = Vel_Matrix_data_2(:,2);
Vel_Y_data_2 = Vel_Matrix_data_2(:,3);
Omega_Z_data_2 = Vel_Matrix_data_2(:,7);
% data 3:
Vel_X_data_3 = Vel_Matrix_data_3(:,2);
Vel_Y_data_3 = Vel_Matrix_data_3(:,3);
Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
%% Plots
%
%
% % marker position
% figure;
% subplot(3,1,1);
% plot(t_P_sec_data_1,Pose_X_data_1);
% hold on
% plot(t_P_sec_data_2,Pose_X_data_2,'r');
% hold on
% plot(t_P_sec_data_3,Pose_X_data_3,'g');
%
% title('Marker X-axis','Color','r');
% % xlabel('Time [sec]');
% ylabel('X [m]');
% grid on
%
% subplot(3,1,2);
% plot(t_P_sec_data_1,Pose_Y_data_1);
% hold on
% plot(t_P_sec_data_2,Pose_Y_data_2,'r');
% hold on
% plot(t_P_sec_data_3,Pose_Y_data_3,'g');
%
% title('Marker Y-axis');
% % xlabel('Time [sec]');
% ylabel('Y [m]');
% grid on
%
% subplot(3,1,3);
% plot(t_P_sec_data_1,Theta_data_1);
% hold on
% plot(t_P_sec_data_2,Theta_data_2,'r');
% hold on
% plot(t_P_sec_data_3,Theta_data_3,'g');
%
% title('Marker \theta-axis');
% xlabel('Time [s]');
% ylabel('\theta [rad]');
% grid on
% control signals
figure;
set(gcf,'color','white');
subplot(3,1,1);
plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',2);
hold on
plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.6);
hold on
plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',2);
title('a) $\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
% set(get(gca,'title'),'Position',([17 5]));
% xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
% grid on
axis([0 35 -.01 .2]);
l = legend('$\dot{x}_{Rob}$ = 0.1 [m/s]','$\dot{x}_{Rob}$ = 0.15[m/s]','$\dot{x}_{Rob}$ = 0.16[m/s]');
set(l,'interpreter','latex','FontSize',12);
legend('boxoff');
subplot(3,1,2);
plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',2.5);
hold on
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.5);
hold on
plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',2.1);
title('a) $\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
% grid on
% axis([0 46 -.2 .1]);
subplot(3,1,3);
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',2.5);
hold on
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.45);
hold on
plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',2.1);
title('a) $\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
xlabel('$ time $ [s]','interpreter','latex','FontSize',14);
ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
% grid on
% axis([0 46 -.1 .15]);
% Trajectory
% figure;
% plot(Pose_Y_data_1,Pose_X_data_1,'b',ref_Y_data_1,ref_X_data_1,'k*');
% hold on
% plot(Pose_Y_data_2,Pose_X_data_2,'r',ref_Y_data_2,ref_X_data_2,'k*');
%
% hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g',ref_Y_data_3,ref_X_data_3,'k*');
%
%
% for k = Pose_X_data_1(1):Pose_X_data_1(size(Pose_X_data_1,1))
% x_arrow(k) =
%
% end
%
% x_arrow = linspace(Pose_X_data_1(1),Pose_X_data_1(size(Pose_X_data_1,1)),150);
% y_arrow = linspace(Pose_Y_data_1(1),Pose_Y_data_1(size(Pose_Y_data_1,1)),150);
%
% Vx = gradient(x_arrow,.01);
% Vy = gradient(y_arrow,.01);
%
% figure;
% plot(y_arrow,x_arrow,'b--','LineWidth',.5);
% % hold on
% % quiver(y_arrow,x_arrow,Vy,Vx,'r--','LineWidth',1.5);
%
figure;
set(gcf,'color','white');
subplot(131);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4)
title('Approach zone + SM zone + Target area');
% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',16);
axis([-.65 .01 -1.47 -.185]);
% grid on
l2 = legend('$\dot{x}_{Rob}$ = 0.1[m/s]','$\dot{x}_{Rob}$ = 0.15[m/s]','$\dot{x}_{Rob}$ = 0.16[m/s]','Target','Location','northwest');
set(l2,'interpreter','latex','FontSize',14);
legend('boxoff');
subplot(1,3,2);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('SM zone + Target area');
xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',16);
% ylabel('${X}$ [m]','interpreter','latex');
axis([-.033 -.01 -.26 -.195]);
% grid on
subplot(133);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('Target area');
% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
% ylabel('${X}$ [m]','interpreter','latex');
axis([-.0205 -.0175 -.206 -.20]);
% grid on
% l2 = legend('x_dot = 0.15','x_dot = 0.1','x_dot = 0.16','Reference Position');
figure;
set(gcf,'color','white');
subplot(2,1,1);
plot(diff(TimeP_ros_data_1)/1e9,'g')
hold on
plot (diff(t_P_sec_data_1),'k--')
title('Position sampling time');
ylabel('Sampling time');
legend('ROS calc.','Real time calc. [sec]','Orientation','horizontal');
% grid on
% axis([0 428 .05 .15]);