diff --git a/Files_4_thesis/Different_angle/docking_different_angle.m b/Files_4_thesis/Different_angle/docking_different_angle.m
index 47ccfddcbce281835b9090d8337e16e7181873ae..5c06cc7ff642d36f066191e42b8ec07ddb3c31c6 100644
--- a/Files_4_thesis/Different_angle/docking_different_angle.m
+++ b/Files_4_thesis/Different_angle/docking_different_angle.m
@@ -155,8 +155,8 @@ hold on
plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.6);
-ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
-title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
+% ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
+% title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);
subplot(323);
@@ -165,8 +165,8 @@ hold on
plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6);
-ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',20);
-title('b) ${y_{mar}}$','interpreter','latex','FontSize',20);
+% ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',20);
+% title('b) ${y_{mar}}$','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);
subplot(325);
@@ -176,9 +176,9 @@ plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.6);
-title('c) ${\theta_{mar}}$','interpreter','latex','FontSize',20);
-ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',20);
-xlabel('$ time$ [s]','interpreter','latex','FontSize',20);
+% title('c) ${\theta_{mar}}$','interpreter','latex','FontSize',20);
+% ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',20);
+% xlabel('$ time$ [s]','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);
% % control signals
@@ -192,9 +192,9 @@ hold on
plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.6);
-
-title('d) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
-ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+%
+% title('d) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
+% ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
l2 = legend('${Left}$', '${Right}$');
set(l2,'interpreter','latex','FontSize',14);
@@ -208,8 +208,8 @@ hold on
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.6);
-ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
-title('e) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
+% ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+% title('e) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);
subplot(326);
@@ -218,9 +218,9 @@ hold on
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.6);
-xlabel('$ time$ [s]','interpreter','latex','FontSize',20);
-ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20);
-title('f) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20);
+% xlabel('$ time$ [s]','interpreter','latex','FontSize',20);
+% ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20);
+% title('f) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);
% Trajectory
@@ -247,8 +247,8 @@ hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
% axis([-.4 .4 .18 1.4]);
-title('${a) Approach + SM + Target}$','interpreter','latex','FontSize',20);
-ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
+% title('${a) Approach + SM + Target}$','interpreter','latex','FontSize',20);
+% ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);
subplot(132);
@@ -260,8 +260,8 @@ plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',1.6);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
-title('$ {b) SM + Target}$','interpreter','latex','FontSize',20);
-xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
+% title('$ {b) SM + Target}$','interpreter','latex','FontSize',20);
+% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
axis([-.03 0.03 -.29 -.191]);
% set(gca,'fontsize',20);
@@ -274,7 +274,7 @@ plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
-title('$ {c) Target}$','interpreter','latex','FontSize',20);
+% title('$ {c) Target}$','interpreter','latex','FontSize',20);
axis([-.0202 -.017 -.2054 -.199]);
% set(gca,'fontsize',20);
diff --git a/Files_4_thesis/Different_controllers/docking_with_PID_controller.asv b/Files_4_thesis/Different_controllers/docking_with_PID_controller.asv
new file mode 100644
index 0000000000000000000000000000000000000000..6d7776c8419247c25c8914df6ba4fb38bbd42022
--- /dev/null
+++ b/Files_4_thesis/Different_controllers/docking_with_PID_controller.asv
@@ -0,0 +1,663 @@
+clc;
+close all;
+clear all;
+
+yaw_off = pi;
+%% Import data and time conversion
+
+% 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);
+
+% 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: 588 ros messages for geometry_msgs/Twist
+% 589 ros messages for geometry_msgs/PoseStamped
+% y-axis gains:
+% P = .5; I = 0.001; D = 0.03;
+
+% 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);
+
+% Pose_Matrix_data_2 = csvread('CAM_PID_.29_.00026_.015.txt',1,0);
+% Vel_Matrix_data_2 = csvread('vel_PID_.29_.00026_.015.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: 538 ros messages for geometry_msgs/Twist
+% 539 ros messages for geometry_msgs/PoseStamped
+% y-axis gains:
+% P = .44; I = .0005; D = 0.15;
+
+% 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);
+
+% Pose_Matrix_data_3 = csvread('Trans_PID_.28_0_.006.txt',1,0);
+% Vel_Matrix_data_3 = csvread('vel_PID_.28_0_.006.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)) = [];
+
+% -----------------------------------------------------------------------------------------------------
+% data 4: 450 ros messages for geometry_msgs/Twist
+% 451 ros messages for geometry_msgs/PoseStamped
+% y-axis gains:
+% P = .86; I = 0; D = 0.002;
+
+% theta-axis gains:
+% P = .08; I = 0; D = 0;
+%
+% Pose_Matrix_data_4 = csvread('POSE_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
+% Vel_Matrix_data_4 = csvread('VEL_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
+
+Pose_Matrix_data_4 = csvread('CAM_PID_.266_.0003_.014.txt',1,0);
+Vel_Matrix_data_4 = csvread('vel_PID_.266_.0003_.014.txt',1,0);
+
+% time in position
+TimeP_ros_data_4 = Pose_Matrix_data_4(:,1); % ros time, needs to be converted to sec...
+
+% duriation = end_time - start_time
+ durationP_data_4 = (TimeP_ros_data_4(size(TimeP_ros_data_4,1),:) - TimeP_ros_data_4(1,:))*10^(-9);
+
+t_P_sec_data_4 = 0:durationP_data_4/size(TimeP_ros_data_4,1):durationP_data_4;
+t_P_sec_data_4(:,size(t_P_sec_data_4,2)) = [];
+
+% time in vel
+TimeV_ros_data_4 = Vel_Matrix_data_4(:,1); % ros time, needs to be converted to sec...
+
+% duriation = end_time - start_time
+ durationV_data_4 = (TimeV_ros_data_4(size(TimeV_ros_data_4,1),:) - TimeV_ros_data_4(1,:))*10^(-9);
+
+t_V_sec_data_4 = 0:durationV_data_4/size(TimeV_ros_data_4,1):durationV_data_4;
+t_V_sec_data_4(:,size(t_V_sec_data_4,2)) = [];
+
+% -----------------------------------------------------------------------------------------------------
+% data 5: ANDROID CAMERA
+
+
+Pose_Matrix_data_5 = csvread('Pose_with_Android.txt',1,0);
+Vel_Matrix_data_5 = csvread('Velocity_with_Android.txt',1,0);
+
+% time in position
+TimeP_ros_data_5 = Pose_Matrix_data_5(:,1); % ros time, needs to be converted to sec...
+
+% duriation = end_time - start_time
+ durationP_data_5 = (TimeP_ros_data_5(size(TimeP_ros_data_5,1),:) - TimeP_ros_data_5(1,:))*10^(-9);
+
+t_P_sec_data_5 = 0:durationP_data_5/size(TimeP_ros_data_5,1):durationP_data_5;
+t_P_sec_data_5(:,size(t_P_sec_data_5,2)) = [];
+
+% time in vel
+TimeV_ros_data_5 = Vel_Matrix_data_5(:,1); % ros time, needs to be converted to sec...
+
+% duriation = end_time - start_time
+ durationV_data_5 = (TimeV_ros_data_5(size(TimeV_ros_data_5,1),:) - TimeV_ros_data_5(1,:))*10^(-9);
+
+t_V_sec_data_5 = 0:durationV_data_5/size(TimeV_ros_data_5,1):durationV_data_5;
+t_V_sec_data_5(:,size(t_V_sec_data_5,2)) = [];
+
+% -----------------------------------------------------------------------------------------------------
+% data 6: with several oscilations
+
+
+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
+TP_ros_6 = Pose_Matrix_data_6(:,1); % ros time, needs to be converted to sec...
+
+% duriation = end_time - start_time
+ duP_6 = (TP_ros_6(size(TP_ros_6,1),:) - TP_ros_6(1,:))*10^(-9);
+
+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
+TV_ros_6 = Vel_Matrix_data_6(:,1); % ros time, needs to be converted to sec...
+
+% duriation = end_time - start_time
+ duV_6 = (TV_ros_6(size(TV_ros_6,1),:) - TV_ros_6(1,:))*10^(-9);
+
+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
+
+%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);
+
+% for i = 1:size(Theta_data_1,1)
+% if (Theta_data_1(i) > 0)
+% Theta_data_1(i) = Theta_data_1(i) - yaw_off;
+% else if(Theta_data_1(i) < 0)
+% Theta_data_1(i) = Theta_data_1(i) + yaw_off;
+% end
+% end
+% end
+% 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);
+
+% for i = 1:size(Theta_data_2,1)
+% if (Theta_data_2(i) > 0)
+% Theta_data_2(i) = Theta_data_2(i) - yaw_off;
+% else if(Theta_data_2(i) < 0)
+% Theta_data_2(i) = Theta_data_2(i) + yaw_off;
+% end
+% end
+% end
+
+% 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);
+
+%
+% for i = 1:size(Theta_data_3,1)
+% if (Theta_data_3(i) > 0)
+% Theta_data_3(i) = Theta_data_3(i) - yaw_off;
+% else if(Theta_data_3(i) < 0)
+% Theta_data_3(i) = Theta_data_3(i) + yaw_off;
+% end
+% end
+% end
+% 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];
+
+%data 4:
+% when using marker pose ,,,,
+Pose_X_data_4 = Pose_Matrix_data_4(:,4);
+Pose_Y_data_4 = Pose_Matrix_data_4(:,3);
+Theta_data_4 = Pose_Matrix_data_4(:,7);
+%
+% for i = 1:size(Theta_data_4,1)
+% if (Theta_data_4(i) > 0)
+% Theta_data_4(i) = Theta_data_4(i) - yaw_off;
+% else if(Theta_data_4(i) < 0)
+% Theta_data_4(i) = Theta_data_4(i) + yaw_off;
+% end
+% end
+% end
+
+% Extracting reference values when the robot is manually docked!
+ref_X_data_4 = Pose_X_data_4(size(Pose_X_data_4,1));
+ref_Y_data_4 = Pose_Y_data_4(size(Pose_Y_data_4,1));
+ref_Theta_data_4 = Theta_data_4(size(Theta_data_4,1));
+ref_Pose_data_4 =[ref_X_data_4;ref_Y_data_4];
+
+
+%data 5:
+% when using marker pose ,,,,
+Pose_X_data_5 = Pose_Matrix_data_5(:,4);
+Pose_Y_data_5 = Pose_Matrix_data_5(:,3);
+Theta_data_5 = Pose_Matrix_data_5(:,7);
+
+% Extracting reference values when the robot is manually docked!
+ref_X_data_5 = Pose_X_data_5(size(Pose_X_data_5,1));
+ref_Y_data_5 = Pose_Y_data_5(size(Pose_Y_data_5,1));
+ref_Theta_data_5 = Theta_data_5(size(Theta_data_5,1));
+ref_Pose_data_5 =[ref_X_data_5;ref_Y_data_5];
+
+%data 6:
+% when using marker pose ,,,,
+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 = 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
+thresh_X = .001;
+theta = 0:.001:2*pi;
+% needs to be adjusted manually if docking platform is replaced!
+ref_x = .2026;
+ref_y = .0087;
+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);
+
+% data 4:
+Vel_X_data_4 = Vel_Matrix_data_4(:,2);
+Vel_Y_data_4 = Vel_Matrix_data_4(:,3);
+Omega_Z_data_4 = Vel_Matrix_data_4(:,7);
+
+% data 5:
+Vel_X_data_5 = Vel_Matrix_data_5(:,2);
+Vel_Y_data_5 = Vel_Matrix_data_5(:,3);
+Omega_Z_data_5 = Vel_Matrix_data_5(:,7);
+
+% data 6:
+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
+
+% % marker position
+% figure;
+% plot(t_P_sec_data_6,Pose_Y_data_6,'k','LineWidth',1.6);
+% xlabel('time');
+% ylabel('y');
+%
+% figure;
+% plot(t_P_sec_data_6,Theta_data_6,'k','LineWidth',1.6);
+% xlabel('time');
+% ylabel('\theta');
+
+% camera pose estimation
+figure;
+set(gcf,'color','white');
+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);
+hold on
+plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.8);
+hold on
+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');
+
+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);
+hold on
+plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.8);
+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(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);
+hold on
+plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.8);
+hold on
+plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.5);
+
+xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
+ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);
+
+
+% control signals
+% figure;
+% set(gcf,'color','white');
+subplot(322);
+
+plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',2.1);
+hold on
+plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.7);
+hold on
+plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.8);
+hold on
+plot(t_V_sec_data_4,Vel_X_data_4,'m','LineWidth',1.5);
+
+title('Control Signals');
+ylabel('$\dot{x}_{Rob}$ [m/s]','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');
+
+
+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);
+hold on
+plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.8);
+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(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);
+hold on
+plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.8);
+hold on
+plot(t_V_sec_data_4,Omega_Z_data_4,'m','LineWidth',1.5);
+xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
+ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
+
+
+% 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(Pose_X_data_1);
+% Vy = gradient(Pose_Y_data_1);
+
+% figure;
+% plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',3.5);
+% hold on
+% quiver(Pose_Y_data_1,Pose_X_data_1,Vy,Vx,'r--','LineWidth',.1);
+
+figure;
+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;
+subplot(1,3,1);
+plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
+% hold on
+% quiver(abs(Pose_Y_data_1),Pose_X_data_1,gradient(Pose_Y_data_1),gradient(Pose_X_data_1),'r--','LineWidth',.1);
+
+hold on
+plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
+% hold on
+% quiver(abs(Pose_Y_data_2),Pose_X_data_2,gradient(Pose_Y_data_2),gradient(Pose_X_data_2),'m--','LineWidth',.1);
+
+hold on
+plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
+% hold on
+% quiver(abs(Pose_Y_data_3),Pose_X_data_3,gradient(Pose_Y_data_3),gradient(Pose_X_data_3),'k--','LineWidth',.1);
+
+hold on
+plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
+% hold on
+% quiver(abs(Pose_Y_data_4),Pose_X_data_4,gradient(Pose_Y_data_4),gradient(Pose_X_data_4),'b--','LineWidth',.1);
+
+hold on
+plot(y_circle,x_circle,'k--','LineWidth',3.4);
+
+title('Approach zone + SM zone + Target area');
+ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',14);
+axis([-.05 .26 .19 1.52]);
+
+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(Pose_Y_data_4,Pose_X_data_4,'m','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',14);
+axis([-.023 0.02 .19 .29]);
+
+subplot(1,3,3);
+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(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
+hold on
+plot(y_circle,x_circle,'k--','LineWidth',3.4);
+
+title('Target area');
+axis([0.0072 .0101 .20 .2056]);
+
+l = 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}$','${Target}$');
+set(l,'interpreter','latex','FontSize',11);
+% legend('boxoff');
+
+
+% % sampling time
+% figure;
+% set(gcf,'color','white');
+%
+% subplot(2,2,1);
+% plot(diff(TimeP_ros_data_3)/1e9,'r','LineWidth',1.4)
+% hold on
+% plot (diff(t_P_sec_data_3),'k--','LineWidth',1.8);
+% % hold on
+% title({'USB Camera';'Position'});
+% ylabel('Sampling time [sec]','interpreter','latex','FontSize',11);
+% axis([0 450 .068 .11]);
+% l3 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
+% set(l3,'interpreter','latex','FontSize',13);
+%
+% subplot(2,2,2);
+% plot(diff(TimeP_ros_data_5)/1e9,'g','LineWidth',1.4)
+% hold on;
+% plot (diff(t_P_sec_data_5),'b--','LineWidth',1.8);
+%
+% title({'Android Camera';'Position'});
+% axis([0 234 .068 .3]);
+%
+% l4 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
+% set(l4,'interpreter','latex','FontSize',13);
+% legend('boxoff');
+%
+% subplot(2,2,3);
+% plot(diff(TimeV_ros_data_3)/1e9,'r','LineWidth',1.4);
+% hold on;
+% plot (diff(t_V_sec_data_3),'k--','LineWidth',1.8);
+% % hold on;
+% title('Velocity');
+% xlabel('samples','interpreter','latex','FontSize',13);
+% ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
+% axis([0 450 .068 .11]);
+%
+% subplot(2,2,4);
+% plot(diff(TimeV_ros_data_5)/1e9,'g','LineWidth',1.4);
+% hold on;
+% plot (diff(t_V_sec_data_5),'b--','LineWidth',1.8);
+%
+% title('Velocity');
+% xlabel('samples','interpreter','latex','FontSize',13);
+% % ylabel('Sampling time [sec]','interpreter','latex','FontSize',11);
+% axis([0 234 .068 .3]);
+%
+
+figure;
+set(gcf,'color','white');
+%
+% subplot(2,1,1);
+pa1 = plot(diff(TimeP_ros_data_3)/1e9,'k','LineWidth',1.5);
+hold on
+pa2 = plot (diff(t_P_sec_data_3),'g--','LineWidth',1.5);
+hold on
+pa3 = plot(diff(TimeP_ros_data_5)/1e9,'b','LineWidth',1.5);
+hold on;
+pa4 = plot (diff(t_P_sec_data_5),'r--','LineWidth',1.8);
+hold off;
+axis([0 234 .05 .3]);
+%
+% title('Position');
+% ylabel('Sampling time [s]','interpreter','latex','FontSize',20);
+% xlabel('Sample number','interpreter','latex','FontSize',20);
+%
+% l5 = legend([pa1 pa2],'$USB Camera {Time_{ROS}}$','$ USB Camera {Time_{Real}} [s]$','Orientation','horizontal');
+% set(l5,'interpreter','latex','FontSize',11);
+% l6 = legend([pa3 pa4],'$ Android Camera {Time_{ROS}}$','$ Android Camera {Time_{Real}} [sec]$','Orientation','horizontal');
+% set(l6,'interpreter','latex','FontSize',12);
+%
+% set(gca,'fontsize',20);
+leg = legend('ROS Time, USB camera','Real Time, USB camera [s]','ROS Time, IP camera','Real Time, IP camera [s]','Location','northwest');
+set(leg,'FontSize',17);
+legend('boxoff');
+
+%
+% subplot(2,1,2);
+% p1 = plot(diff(TimeV_ros_data_3)/1e9,'r','LineWidth',2.1);
+% hold on;
+% p2 = plot (diff(t_V_sec_data_3),'k--','LineWidth',1.5);
+% hold on;
+% p3 = plot(diff(TimeV_ros_data_5)/1e9,'g','LineWidth',1.4);
+% hold on;
+% p4 = plot (diff(t_V_sec_data_5),'b--','LineWidth',1.8);
+% hold off;
+% axis([0 234 0 .3]);
+% title('Velocity');
+% ylabel('Sampling time [s]','interpreter','latex','FontSize',13);
+% xlabel('Sample','interpreter','latex','FontSize',13);
+%
+% set(gca,'fontsize',13);
+
+
+MaxOvershoot_1 = -ref_y + min(Pose_Y_data_1)
+MaxOvershoot_2 = -ref_y + min(Pose_Y_data_2)
+MaxOvershoot_3 = -ref_y + min(Pose_Y_data_3)
+MaxOvershoot_4 = -ref_y + min(Pose_Y_data_4)
+% Pose diff
+% figure;
+% subplot(3,1,1);
+% plot(diff(Pose_X_data_1));
+% title('X-Pose diff');
+% xlabel('samples');
+% ylabel('X difference [m]');
+% grid on
+%
+% subplot(3,1,2);
+% plot(diff(Pose_X_data_1));
+% title('Y-Pose diff');
+% xlabel('samples');
+% ylabel('Y difference [m]');
+% grid on
+%
+% subplot(3,1,3);
+% plot(diff(Theta_data_1));
+% title('\theta-Pose diff');
+% xlabel('samples');
+% ylabel('\theta difference [rad]');
+% grid on
\ No newline at end of file
diff --git a/Files_4_thesis/Different_controllers/docking_with_PID_controller.m b/Files_4_thesis/Different_controllers/docking_with_PID_controller.m
index 4ad2bf4b7756e077e5a4ff9f79667a92c9e24437..6a01524dedef564bb4915fe1f4922ab4ff4b9755 100644
--- a/Files_4_thesis/Different_controllers/docking_with_PID_controller.m
+++ b/Files_4_thesis/Different_controllers/docking_with_PID_controller.m
@@ -9,11 +9,11 @@ yaw_off = pi;
% 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('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);
+% 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);
% time in position
TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
@@ -41,12 +41,12 @@ 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);
%
-Pose_Matrix_data_2 = csvread('CAM_PID_.29_.00026_.015.txt',1,0);
-Vel_Matrix_data_2 = csvread('vel_PID_.29_.00026_.015.txt',1,0);
+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);
+
+% Pose_Matrix_data_2 = csvread('CAM_PID_.29_.00026_.015.txt',1,0);
+% Vel_Matrix_data_2 = csvread('vel_PID_.29_.00026_.015.txt',1,0);
% time in position
TimeP_ros_data_2 = Pose_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
@@ -74,12 +74,12 @@ 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);
-Pose_Matrix_data_3 = csvread('Trans_PID_.28_0_.006.txt',1,0);
-Vel_Matrix_data_3 = csvread('vel_PID_.28_0_.006.txt',1,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);
+
+% Pose_Matrix_data_3 = csvread('Trans_PID_.28_0_.006.txt',1,0);
+% Vel_Matrix_data_3 = csvread('vel_PID_.28_0_.006.txt',1,0);
% time in position
TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
@@ -107,12 +107,12 @@ t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];
% theta-axis gains:
% P = .08; I = 0; D = 0;
-%
-% Pose_Matrix_data_4 = csvread('POSE_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
-% Vel_Matrix_data_4 = csvread('VEL_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
-Pose_Matrix_data_4 = csvread('CAM_PID_.266_.0003_.014.txt',1,0);
-Vel_Matrix_data_4 = csvread('vel_PID_.266_.0003_.014.txt',1,0);
+Pose_Matrix_data_4 = csvread('POSE_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
+Vel_Matrix_data_4 = csvread('VEL_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
+
+% Pose_Matrix_data_4 = csvread('CAM_PID_.266_.0003_.014.txt',1,0);
+% Vel_Matrix_data_4 = csvread('vel_PID_.266_.0003_.014.txt',1,0);
% time in position
TimeP_ros_data_4 = Pose_Matrix_data_4(:,1); % ros time, needs to be converted to sec...
diff --git a/Files_4_thesis/scanner_analysis.m b/Files_4_thesis/scanner_analysis.m
index ce86cc66557426bffd9791a2af3f4689af8b52d5..426d55297e060a076fc81f3ffa7bf294034b2a1e 100644
--- a/Files_4_thesis/scanner_analysis.m
+++ b/Files_4_thesis/scanner_analysis.m
@@ -167,7 +167,6 @@ for i = 1:points_DockedwithBOX
Y_obst_DockedwithBOX(i)= dist_to_obst * sin(s_angle_DockedwithBOX(i));
end
-
% checking accuracy in robot coordinate system
figure;
set(gcf,'color','white');
@@ -197,7 +196,7 @@ orig_coordinate_cir = gca; % create axes
plot(orig_coordinate_cir,s_angle_DockedwithMar,s_val_DockedwithMar,'b','LineWidth',2.7);
hold on
plot(orig_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',2.7);
-ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',20);
+% ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',20);
axis([-2.3 2.3 -.5 10]);
% set(orig_coordinate_cir,'box','off','fontsize',20);
set(orig_coordinate_cir,'box','off');
@@ -224,42 +223,41 @@ set(zoomed_coordinate_cir,'ylim',[.08 .2])% axes limit for zoom out,
set(zoomed_coordinate_cir, 'Units', 'normalized', 'Position', [.35 0.48 0.15 0.18])
% ----- Zoomed in ------
-subplot(324);
-orig_coordinate_cir_T = gca; % create axes
-plot(orig_coordinate_cir_T,X_DockedwithMar,Y_DockedwithMar,'b','LineWidth',2.7);
-hold on;
-plot(orig_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
-ylabel('$ {Y} $ [m]','interpreter','latex','FontSize',20);
-axis([-2 4.5 -4 1.5]);
-% set(orig_coordinate_cir_T,'box','off','fontsize',20);
-set(orig_coordinate_cir_T,'box','off');
-
-% ----- Zoomed in ------
-hkids_cir_T = get(orig_coordinate_cir_T, 'child'); % get original coordinate properties
+% subplot(324);
+% orig_coordinate_cir_T = gca; % create axes
+% plot(orig_coordinate_cir_T,X_DockedwithMar,Y_DockedwithMar,'b','LineWidth',2.7);
+% hold on;
+% plot(orig_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
+% % ylabel('$ {Y} $ [m]','interpreter','latex','FontSize',20);
+% axis([-2 4.5 -4 1.5]);
+% % set(orig_coordinate_cir_T,'box','off','fontsize',20);
+% set(orig_coordinate_cir_T,'box','off');
%
-% set(hkids_cir_T(1),'marker','.','Color','r')
-% set(hkids_cir_T(2),'marker','*')
-
-set(hkids_cir_T(1),'Color','r')
-set(hkids_cir_T(2),'Color','b')
-
-
-zoomed_coordinate_cir_T = axes('Position',get(orig_coordinate_cir_T,'Position'),...
- 'XAxisLocation','bottom',...
- 'YAxisLocation','left',...
- 'fontsize',10,...
- 'Color','none',...
- 'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings
-
-plot(zoomed_coordinate_cir_T,X_DockedwithMar,Y_DockedwithMar,'b','LineWidth',2.7);
-hold on;
-plot(zoomed_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
-
-set(zoomed_coordinate_cir_T,'xlim',[.08 .13])% axes limit for zoom out,
-set(zoomed_coordinate_cir_T,'ylim',[-.06 .06])% axes limit for zoom out,
-set(zoomed_coordinate_cir_T, 'Units', 'normalized', 'Position', [.84 0.485 0.15 0.18])
-% ----- Zoomed in ------
-
+% % ----- Zoomed in ------
+% hkids_cir_T = get(orig_coordinate_cir_T, 'child'); % get original coordinate properties
+% %
+% % set(hkids_cir_T(1),'marker','.','Color','r')
+% % set(hkids_cir_T(2),'marker','*')
+%
+% set(hkids_cir_T(1),'Color','r')
+% set(hkids_cir_T(2),'Color','b')
+%
+%
+% zoomed_coordinate_cir_T = axes('Position',get(orig_coordinate_cir_T,'Position'),...
+% 'XAxisLocation','bottom',...
+% 'YAxisLocation','left',...
+% 'fontsize',10,...
+% 'Color','none',...
+% 'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings
+%
+% plot(zoomed_coordinate_cir_T,X_DockedwithMar,Y_DockedwithMar,'b','LineWidth',2.7);
+% hold on;
+% plot(zoomed_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
+%
+% set(zoomed_coordinate_cir_T,'xlim',[.08 .13])% axes limit for zoom out,
+% set(zoomed_coordinate_cir_T,'ylim',[-.06 .06])% axes limit for zoom out,
+% set(zoomed_coordinate_cir_T, 'Units', 'normalized', 'Position', [.84 0.485 0.15 0.18])
+% % ----- Zoomed in ------
subplot(325);
@@ -267,7 +265,7 @@ orig_coordinate_box = gca; % create axes
plot(orig_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',2.7);
hold on
plot(orig_coordinate_box,s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',3);
-xlabel('$ {\theta_b} $ [rad]','interpreter','latex','FontSize',20);
+% xlabel('$ {\theta_b} $ [rad]','interpreter','latex','FontSize',20);
axis([-2.3 2.3 -.15 5]);
% set(orig_coordinate_box,'box','off','fontsize',20);
set(orig_coordinate_box,'box','off');
@@ -300,7 +298,7 @@ orig_coordinate_box_T = gca; % create axes
plot(orig_coordinate_box_T,X_DockedwithBOX,Y_DockedwithBOX,'b','LineWidth',2.7);
hold on
plot(orig_coordinate_box_T,X_obst_DockedwithMar,Y_obst_DockedwithMar, 'r','LineWidth',3);
-xlabel('$ {X} $ [m]','interpreter','latex','FontSize',20);
+% xlabel('$ {X} $ [m]','interpreter','latex','FontSize',20);
axis([-.53 .9 -.8 1.5]);
% set(orig_coordinate_box_T,'box','off','fontsize',20);
set(orig_coordinate_box_T,'box','off');
diff --git a/Files_4_thesis/x_tANDy_t/y_tANDx_t.m b/Files_4_thesis/x_tANDy_t/y_tANDx_t.m
index cf1c6fa8d37d5133445c179d992107c6b010bc8d..dc0006f8dcd66f58710797dc3248ed057cebfea4 100644
--- a/Files_4_thesis/x_tANDy_t/y_tANDx_t.m
+++ b/Files_4_thesis/x_tANDy_t/y_tANDx_t.m
@@ -263,8 +263,9 @@ plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.7);
hold on
plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.6);
% set(gca,'fontsize',20);
-ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
-title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
+
+% ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
+% title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
%
% l2 = legend('${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$', '${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$');
% set(l2,'interpreter','latex','FontSize',9);
@@ -281,8 +282,8 @@ plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.7);
hold on
plot(t_P_sec_data_4,Pose_Y_data_4,'m','LineWidth',1.6);
% set(gca,'fontsize',20);
-ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',20);
-title('b) ${y_{mar}}$','interpreter','latex','FontSize',20);
+% ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',20);
+% title('b) ${y_{mar}}$','interpreter','latex','FontSize',20);
% subplot(338);
subplot(325);
@@ -295,9 +296,9 @@ plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.7);
hold on
plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.6);
% set(gca,'fontsize',20);
-title('c) ${\theta_{mar}}$','interpreter','latex','FontSize',20);
-xlabel('$ time [s]$','interpreter','latex','FontSize',20);
-ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',20);
+% title('c) ${\theta_{mar}}$','interpreter','latex','FontSize',20);
+% xlabel('$ time [s]$','interpreter','latex','FontSize',20);
+% ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',20);
% Control Signals
@@ -317,8 +318,8 @@ plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.7);
hold on
plot(t_V_sec_data_4,Vel_X_data_4,'m','LineWidth',1.6);
% set(gca,'fontsize',20);
-title('d) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
-ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+% title('d) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
+% ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
%
% l2 = legend('${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$', '${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$');
% set(l2,'interpreter','latex','FontSize',13);
@@ -336,8 +337,8 @@ plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.7);
hold on
plot(t_V_sec_data_4,Vel_Y_data_4,'m','LineWidth',1.6);
% set(gca,'fontsize',20);
-ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
-title('e) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
+% ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+% title('e) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
% subplot(339);
subplot(326);
@@ -350,9 +351,9 @@ plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.7);
hold on
plot(t_V_sec_data_4,Omega_Z_data_4,'m','LineWidth',1.6);
% set(gca,'fontsize',20);
-title('f) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20);
-xlabel('$ time [s]$','interpreter','latex','FontSize',20);
-ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20);
+% title('f) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20);
+% xlabel('$ time [s]$','interpreter','latex','FontSize',20);
+% ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20);
% Trajectory
@@ -381,8 +382,8 @@ plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
% set(gca,'fontsize',20);
-title('${a) Approach + SM + Target}$','interpreter','latex','FontSize',20);
-ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
+% title('${a) Approach + SM + Target}$','interpreter','latex','FontSize',20);
+% ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
% axis([-.65 .05 -1.6 -.19]);
axis([-.65 .03 -1.47 -.16]);
subplot(132);
@@ -396,8 +397,8 @@ plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
% set(gca,'fontsize',20);
-title('$ {b) SM + Target}$','interpreter','latex','FontSize',20);
-xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
+% title('$ {b) SM + Target}$','interpreter','latex','FontSize',20);
+% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
axis([-.025 0.005 -.29 -.19]);
% axis([-.033 -.01 -.26 -.195]);
subplot(133);
@@ -411,7 +412,7 @@ plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
% set(gca,'fontsize',20);
-title('$ {c) Target}$','interpreter','latex','FontSize',20);
+% title('$ {c) Target}$','interpreter','latex','FontSize',20);
% axis([-.0205 -.0168 -.2054 -.199]);
axis([-.0201 -.017 -.206 -.1995]);
l = legend('${P_y = .266, I_y = .0003, D_y = .014}$', '${P_y = .28, I_y = 0, D_y = .006}$','${P_y = .29, I_y = .00026, D_y = .015}$','${P_y = .3, I_y = .0004, D_y = .02}$','${Target}$');
diff --git a/Paper/IREIT/Paper_IREIT.docx b/Paper/IREIT/Paper_IREIT.docx
index ccac33f8fa46668b19f1158c9eff3c20416abc95..117c15c1786ec80c85c1228c9ce8048b4fec9253 100644
Binary files a/Paper/IREIT/Paper_IREIT.docx and b/Paper/IREIT/Paper_IREIT.docx differ
diff --git a/Paper/IREIT/Paper_IREIT.pdf b/Paper/IREIT/Paper_IREIT.pdf
index a546cd6d9bbf1a68f80352b8dfe1948ee31edcad..ddbade453cfe210107485736bfd15c4d074ee29a 100644
Binary files a/Paper/IREIT/Paper_IREIT.pdf and b/Paper/IREIT/Paper_IREIT.pdf differ
diff --git a/Paper/IREIT/~$per_IREIT.docx b/Paper/IREIT/~$per_IREIT.docx
new file mode 100644
index 0000000000000000000000000000000000000000..62f908ea0e9ac7185970d20bd005b35d536f7e70
Binary files /dev/null and b/Paper/IREIT/~$per_IREIT.docx differ