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..9a382908b10e95ae252a188b32e9fb79862150eb --- /dev/null +++ b/Files_4_thesis/Different_controllers/docking_with_PID_controller.asv @@ -0,0 +1,648 @@ +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 +TimeP_ros_data_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); + +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)) = []; + +% time in vel +TimeV_ros_data_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); + +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)) = []; + + +%% 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 ,,,, +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); + +% 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_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: +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); + +%% 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'); + + +figure; +set(gcf,'color','white'); +subplot(3,1,1); +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(3,1,2); +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(3,1,3); +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(3,1,1); + +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(3,1,2); +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(3,1,3); +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(Pose_Y_data_6,Pose_X_data_6,'k','LineWidth',1.4); +axis([-.3 .7 .1 1.6]); + + +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',2.1); +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.4); +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',19); +xlabel('Sample number','interpreter','latex','FontSize',19); +% +% 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',17); +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',13); +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 0198c84553e4ea238fc5046296641acfae37956c..1effce590e4d3051abc28642ef8ddbb197a10a22 100644 --- a/Files_4_thesis/Different_controllers/docking_with_PID_controller.m +++ b/Files_4_thesis/Different_controllers/docking_with_PID_controller.m @@ -335,16 +335,16 @@ Omega_Z_data_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'); +% % 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'); figure; @@ -526,93 +526,99 @@ 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); +% % sampling time +% figure; +% set(gcf,'color','white'); +% +% subplot(2,2,1); +% plot(diff(TimeP_ros_data_3)/1e9,'r','LineWidth',1.4) % 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); +% 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 234 .068 .3]); - +% 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,'r','LineWidth',2.1); +% +% 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),'k--','LineWidth',1.5); +pa2 = plot (diff(t_P_sec_data_3),'g--','LineWidth',1.5); hold on -pa3 = plot(diff(TimeP_ros_data_5)/1e9,'g','LineWidth',1.4); +pa3 = plot(diff(TimeP_ros_data_5)/1e9,'b','LineWidth',1.5); hold on; -pa4 = plot (diff(t_P_sec_data_5),'b--','LineWidth',1.8); +pa4 = plot (diff(t_P_sec_data_5),'r--','LineWidth',1.8); hold off; -axis([0 234 0 .3]); -title('Position'); -% ylabel('Sampling time [sec]','interpreter','latex','FontSize',13); +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}} [sec]$','Orientation','horizontal'); +% l5 = legend([pa1 pa2],'$USB Camera {Time_{ROS}}$','$ USB Camera {Time_{Real}} [s]$','Orientation','horizontal'); % set(l5,'interpreter','latex','FontSize',11); - -% leg = -legend('ROS Time, USB camera','Real Time, USB camera [s]','ROS Time, IP camera','Real Time, IP camera [s]','Location','northwest'); +% 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',15); 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 [sec]','interpreter','latex','FontSize',13); +% +% 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); % -% l6 = legend([p3 p4],'$ Android Camera {Time_{ROS}}$','$ Android Camera {Time_{Real}} [sec]$','Orientation','horizontal'); -% set(l6,'interpreter','latex','FontSize',12); +% set(gca,'fontsize',13); + MaxOvershoot_1 = -ref_y + min(Pose_Y_data_1) MaxOvershoot_2 = -ref_y + min(Pose_Y_data_2) diff --git a/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv b/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv new file mode 100644 index 0000000000000000000000000000000000000000..3a19580683d35af3496a2bffffbc179178cbe5e6 --- /dev/null +++ b/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv @@ -0,0 +1,400 @@ +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]); + +subplot(2,1,2); +plot(diff(TimeV_ros_data_1)/1e9,'g'); +hold on +plot (diff(t_V_sec_data_1),'k--') +title('Velocity sampling time'); +xlabel('samples'); +ylabel('Sampling time'); +% legend('ROS calc.','Ordinary calc.','Orientation','horizontal'); +% grid on +% axis([0 428 -.005 .025]); +% +% subplot(3,1,3); +% plot(diff(TimeV_ros_data_1)/1e9,'g') +% hold on +% plot (diff(t_V_sec_data_1),'k--') +% xlabel('samples'); +% ylabel('time step'); +% axis([150 250 0 .02]); +% figure; +% set(gcf,'color','white'); +% +% 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/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m b/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m index d9b541c7506fb48faad29953248766c33d5bdc6c..9aa19655367fe866d9e5b052f6031566766b241f 100644 --- a/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m +++ b/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m @@ -176,45 +176,45 @@ 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 +% +% +% % 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 @@ -229,13 +229,14 @@ 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('Control Signals'); +title('a) $\dot{x}_{Rob}$','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',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); +set(l,'interpreter','latex','FontSize',18); legend('boxoff'); subplot(3,1,2); @@ -244,9 +245,8 @@ 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('Control Signal y_{Rob} - axis'); -ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14); +title('b) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20); +ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20); % grid on % axis([0 46 -.2 .1]); @@ -256,16 +256,14 @@ 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('Control Signal \theta_{Rob} - axis'); -xlabel('$ time $ [s]','interpreter','latex','FontSize',14); -ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14); +title('c) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20); +xlabel('$ time $ [s]','interpreter','latex','FontSize',20); +ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20); % grid on % axis([0 46 -.1 .15]); - % Trajectory % figure; @@ -306,10 +304,9 @@ 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'); +title('a) Approach zone + SM zone + Target area','FontSize',17); % xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14); -ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',16); +ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20); 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'); @@ -324,8 +321,8 @@ 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); +title('b) SM zone + Target area','FontSize',17); +xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20); % ylabel('${X}$ [m]','interpreter','latex'); axis([-.033 -.01 -.26 -.195]); % grid on @@ -338,7 +335,7 @@ 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'); +title('c) Target area','FontSize',17); % xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14); % ylabel('${X}$ [m]','interpreter','latex'); axis([-.0205 -.0175 -.206 -.20]); diff --git a/Files_4_thesis/Report_with_updated_plots.docx b/Files_4_thesis/Report_with_updated_plots.docx index 6a5a6c9e27327b47496f51d29a35d6dd88515cc1..7e90183996c4256ed1661ee3edb7fbcc864ed6c8 100644 Binary files a/Files_4_thesis/Report_with_updated_plots.docx and b/Files_4_thesis/Report_with_updated_plots.docx differ diff --git a/Files_4_thesis/UP_REP_Farid_Alijani.pdf b/Files_4_thesis/UP_REP_Farid_Alijani.pdf index 0b5cb49fdfbfca996acf77aed4165dc827fb537e..ef29a9eb61244b667ed8c6cfb4c367c276baa865 100644 Binary files a/Files_4_thesis/UP_REP_Farid_Alijani.pdf and b/Files_4_thesis/UP_REP_Farid_Alijani.pdf differ diff --git a/Files_4_thesis/scanner_analysis.asv b/Files_4_thesis/scanner_analysis.asv new file mode 100644 index 0000000000000000000000000000000000000000..025b70470ae2b206f7b73bff3ac4ac7beff576bc --- /dev/null +++ b/Files_4_thesis/scanner_analysis.asv @@ -0,0 +1,402 @@ +clc; +close all; +clear all; + +upper_angle = 135; % deg. +lower_angle = -135; % deg. +dist_to_obst = .1; % m. + +%% docked robot for 5 different experiences... + +S_mat = csvread('Scan_Data_5_exp.csv'); + +indx = S_mat(:,1); + +s_exp_1 = S_mat(:,2); +s_exp_2 = S_mat(:,3); +s_exp_3 = S_mat(:,4); +s_exp_4 = S_mat(:,5); +s_exp_5 = S_mat(:,6); + + +points = size(indx,1); % to calculate the angle... +laser_beam = points -1; + +angle = zeros(points,1); +X_exp_1 = zeros(points,1); +X_exp_2 = zeros(points,1); +X_exp_3 = zeros(points,1); +X_exp_4 = zeros(points,1); +X_exp_5 = zeros(points,1); +X_obst = zeros(points,1); + +Y_exp_1 = zeros(points,1); +Y_exp_2 = zeros(points,1); +Y_exp_3 = zeros(points,1); +Y_exp_4 = zeros(points,1); +Y_exp_5 = zeros(points,1); +Y_obst = zeros(points,1); + + + +for i = 1:points + + angle(i) = (lower_angle + (indx(i)* ((upper_angle - lower_angle)/(laser_beam))))* pi/180; + + X_exp_1(i) = s_exp_1(i) * cos(angle(i)); + X_exp_2(i) = s_exp_2(i) * cos(angle(i)); + X_exp_3(i) = s_exp_3(i) * cos(angle(i)); + X_exp_4(i) = s_exp_4(i) * cos(angle(i)); + X_exp_5(i) = s_exp_5(i) * cos(angle(i)); + X_obst(i) = dist_to_obst * cos(angle(i)); + + Y_exp_1(i) = s_exp_1(i) * sin(angle(i)); + Y_exp_2(i) = s_exp_2(i) * sin(angle(i)); + Y_exp_3(i) = s_exp_3(i) * sin(angle(i)); + Y_exp_4(i) = s_exp_4(i) * sin(angle(i)); + Y_exp_5(i) = s_exp_5(i) * sin(angle(i)); + Y_obst(i) = dist_to_obst * sin(angle(i)); +end + +% theta vs. scanner_value & reference +figure; +set(gcf,'color','white'); +subplot(1,2,1); + +% plot(angle,s_exp_1,'m--','LineWidth',1.1); +% hold on; +plot(angle,s_exp_2,'m--','LineWidth',1.9); +hold on; +plot(angle,s_exp_3,'b--','LineWidth',1.9); +hold on; +plot(angle,s_exp_4,'g--','LineWidth',1.1); +hold on; +% plot(angle,s_exp_5,'b--','LineWidth',.8); +% hold on; +plot(angle,dist_to_obst,'rx','LineWidth',.05); +title(''); +xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',20); +ylabel('${Distance}$ [m]','interpreter','latex','FontSize',20); +axis([-.09 .42 .08 .2]); +set(gca,'box','off','fontsize',20); +% legend('Experiment 1','Experiment 2','Experiment 3','Safety Curve'); +subplot(1,2,2); +% plot(X_exp_1,Y_exp_1,'m--','LineWidth',.9); +% hold on +plot(X_exp_2,Y_exp_2,'m--','LineWidth',1.9); +hold on +plot(X_exp_3,Y_exp_3,'b--','LineWidth',1.9); +hold on +plot(X_exp_4,Y_exp_4,'g--','LineWidth',1.1); +hold on +% plot(X_exp_5,Y_exp_5,'b--','LineWidth',.1); +% hold on +plot(X_obst,Y_obst,'r--','LineWidth',.05); +title(''); +xlabel('${X}$ [m]','interpreter','latex','FontSize',20); +ylabel('${Y}$ [m]','interpreter','latex','FontSize',20); +axis([-.1 .16 -.18 .18]); +set(gca,'box','off','fontsize',20); +l = legend('Trial 1','Trial 2','Trial 3','Safety Curve','Orientation','horizontal'); +set(l,'interpreter','latex','FontSize',13); + +%% docked robot with marker vs. docked robot without marker! +S_Mat_DockedwithMar = csvread('scanner_data_Docked_WITH_Marker.txt',0,0); +S_Mat_DockedwithNOmar = csvread('scanner_data_no_cylinder.txt',0,0); + +indx_DockedwithMar = S_Mat_DockedwithMar(:,1); +s_val_DockedwithMar = S_Mat_DockedwithMar(:,2); + +indx_DockedwithNOmar = S_Mat_DockedwithNOmar(:,1); +s_val_DockedwithNOmar = S_Mat_DockedwithNOmar(:,2); + +points_DockedwithMar = size(indx_DockedwithMar,1); +points_DockedwithNOmar = size(indx_DockedwithNOmar,1); + +laser_beam_DockedwithMar = points_DockedwithMar - 1; +laser_beam_DockedwithNOmar = points_DockedwithNOmar - 1; + +s_angle_DockedwithMar = zeros(points_DockedwithMar,1); +s_angle_DockedwithNOmar = zeros(points_DockedwithNOmar,1); + +X_DockedwithMar = zeros(points_DockedwithMar,1); Y_DockedwithMar = zeros(points_DockedwithMar,1); +X_DockedwithNOmar = zeros(points_DockedwithNOmar,1); Y_DockedwithNOmar = zeros(points_DockedwithNOmar,1); + +X_obst_DockedwithMar = zeros(points_DockedwithMar,1); Y_obst_DockedwithMar = zeros(points_DockedwithMar,1); +X_obst_DockedwithNOmar = zeros(points_DockedwithNOmar,1); Y_obst_DockedwithNOmar = zeros(points_DockedwithNOmar,1); + + +for i = 1:points_DockedwithMar + s_angle_DockedwithMar(i) = (lower_angle + (indx_DockedwithMar(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithMar))))* pi/180; + X_DockedwithMar(i) = s_val_DockedwithMar(i) * cos(s_angle_DockedwithMar(i)); + Y_DockedwithMar(i) = s_val_DockedwithMar(i) * sin(s_angle_DockedwithMar(i)); + X_obst_DockedwithMar(i) = dist_to_obst * cos(s_angle_DockedwithMar(i)); + Y_obst_DockedwithMar(i)= dist_to_obst * sin(s_angle_DockedwithMar(i)); +end + + +for i = 1:points_DockedwithNOmar + s_angle_DockedwithNOmar(i) = (lower_angle + (indx_DockedwithNOmar(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithNOmar))))* pi/180; + X_DockedwithNOmar(i) = s_val_DockedwithNOmar(i) * cos(s_angle_DockedwithNOmar(i)); + Y_DockedwithNOmar(i) = s_val_DockedwithNOmar(i) * sin(s_angle_DockedwithNOmar(i)); + X_obst_DockedwithNOmar(i) = dist_to_obst * cos(s_angle_DockedwithNOmar(i)); + Y_obst_DockedwithNOmar(i)= dist_to_obst * sin(s_angle_DockedwithNOmar(i)); +end + + + +% The robot is totally docked and a box is besides of laser scanner! +S_Mat_DockedwithBOX = csvread('scanner_data_box.txt',0,0); +indx_DockedwithBOX = S_Mat_DockedwithBOX(:,1); +s_val_DockedwithBOX = S_Mat_DockedwithBOX(:,2); + +points_DockedwithBOX = size(indx_DockedwithBOX,1); +laser_beam_DockedwithBOX = points_DockedwithBOX - 1; +s_angle_DockedwithBOX = zeros(points_DockedwithBOX,1); + +X_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_DockedwithBOX = zeros(points_DockedwithBOX,1); +X_obst_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_obst_DockedwithBOX = zeros(points_DockedwithBOX,1); + +for i = 1:points_DockedwithBOX + s_angle_DockedwithBOX(i) = (lower_angle + (indx_DockedwithBOX(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithBOX))))* pi/180; + X_DockedwithBOX(i) = s_val_DockedwithBOX(i) * cos(s_angle_DockedwithBOX(i)); + Y_DockedwithBOX(i) = s_val_DockedwithBOX(i) * sin(s_angle_DockedwithBOX(i)); + + + X_obst_DockedwithBOX(i) = dist_to_obst * cos(s_angle_DockedwithBOX(i)); + Y_obst_DockedwithBOX(i)= dist_to_obst * sin(s_angle_DockedwithBOX(i)); +end + + +% checking accuracy in robot coordinate system +figure; +set(gcf,'color','white'); + +subplot(3,2,1); +plot(s_angle_DockedwithNOmar,s_val_DockedwithNOmar); +hold on +plot(s_angle_DockedwithNOmar,dist_to_obst,'r','LineWidth',.2); +% title('No marker on docking platform'); +axis([-2.2 2.2 -.5 10]); +set(gca,'box','off','fontsize',20); + +subplot(3,2,2); +plot(X_DockedwithNOmar,Y_DockedwithNOmar); +hold on; +plot(X_obst_DockedwithNOmar,Y_obst_DockedwithNOmar,'r','LineWidth',3); +% title('No marker on docking platform, TOP VIEW'); +% legend('Obstacles','Laser scanner safety curve (r = 10 cm)'); +% grid on +axis([-2 4.5 -4 1.5]); +set(gca,'box','off','fontsize',20); + + +subplot(3,2,3); +orig_coordinate_cir = gca; % create axes +plot(orig_coordinate_cir,s_angle_DockedwithMar,s_val_DockedwithMar); +hold on +plot(orig_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',.2); +ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',14); +axis([-2.3 2.3 -.5 10]); +set(orig_coordinate_cir,'box','off','fontsize',20); + +% ----- Zoomed in ------ +hkids_cir = get(orig_coordinate_cir, 'child'); % get original coordinate properties + +set(hkids_cir(1),'marker','o','Color','r') +set(hkids_cir(2),'marker','.') + +zoomed_coordinate_cir = axes('Position',get(orig_coordinate_cir,'Position'),... + 'XAxisLocation','bottom',... + 'YAxisLocation','left',... + 'fontsize',15,... + 'Color','none',... + 'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings + +plot(zoomed_coordinate_cir,s_angle_DockedwithMar,s_val_DockedwithMar); +hold on; +plot(zoomed_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'Color','r'); + +set(zoomed_coordinate_cir,'xlim',[-.15 .32])% axes limit for zoom out, +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(3,2,4); +orig_coordinate_cir_T = gca; % create axes +plot(orig_coordinate_cir_T,X_DockedwithMar,Y_DockedwithMar); +hold on; +plot(orig_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3); +ylabel('$ {Y} $ [m]','interpreter','latex','FontSize',14); +axis([-2 4.5 -4 1.5]); +set(orig_coordinate_cir_T,'box','off','fontsize',20); + +% ----- 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',13,... + '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); +hold on; +plot(zoomed_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'Color','r'); + +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(3,2,5); +orig_coordinate_box = gca; % create axes +plot(orig_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01); +hold on +plot(orig_coordinate_box,s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5); +xlabel('$ {\theta_b} $ [rad]','interpreter','latex','FontSize',14); +axis([-2.3 2.3 -.15 5]); +set(orig_coordinate_box,'box','off','fontsize',20); + +% ----- Zoomed in BOX------ +hkids_box = get(orig_coordinate_box, 'child'); % get original coordinate properties + +set(hkids_box(1),'marker','o','Color','r') +set(hkids_box(2),'marker','.') + +zoomed_coordinate_box = axes('Position',get(orig_coordinate_box,'Position'),... + 'XAxisLocation','bottom',... + 'YAxisLocation','left',... + 'fontsize',13,... + 'Color','none',... + 'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings + +plot(zoomed_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX); +hold on; +plot(zoomed_coordinate_box,s_angle_DockedwithBOX,dist_to_obst,'Color','r'); + +set(zoomed_coordinate_box,'xlim',[-1.6 -.3])% axes limit for zoom out, +set(zoomed_coordinate_box,'ylim',[.08 .2])% axes limit for zoom out, +set(zoomed_coordinate_box, 'Units', 'normalized', 'Position', [.35 0.18 0.15 0.18]) +% ----- Zoomed in BOX------ + + +subplot(3,2,6); +orig_coordinate_box_T = gca; % create axes +plot(orig_coordinate_box_T,X_DockedwithBOX,Y_DockedwithBOX,'b','LineWidth',.01); +hold on +plot(orig_coordinate_box_T,X_obst_DockedwithMar,Y_obst_DockedwithMar, 'r','LineWidth',5); +xlabel('$ {X} $ [m]','interpreter','latex','FontSize',14); +axis([-.53 .9 -.8 1.5]); +set(orig_coordinate_box_T,'box','off','fontsize',20); + +% ----- Zoomed in BOX_T------ +hkids_box_T = get(orig_coordinate_box_T, 'child'); % get original coordinate properties + +set(hkids_box_T(1),'Color','r') +set(hkids_box_T(2),'Color','b') + +zoomed_coordinate_box_T = axes('Position',get(orig_coordinate_box_T,'Position'),... + 'XAxisLocation','bottom',... + 'YAxisLocation','left',... + 'fontsize',13,... + 'Color','none',... + 'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings + +plot(zoomed_coordinate_box_T,X_DockedwithBOX,Y_DockedwithBOX); +hold on; +plot(zoomed_coordinate_box_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'Color','r'); + +set(zoomed_coordinate_box_T,'xlim',[-.16 .2])% axes limit for zoom out, +set(zoomed_coordinate_box_T,'ylim',[-.22 .22])% axes limit for zoom out, +set(zoomed_coordinate_box_T, 'Units', 'normalized', 'Position', [.84 0.2 .15 .18]) +% ----- Zoomed in BOX_T------ + + +%% The robot is moving Towards docking...! +Scanner_Matrix_Moving = csvread('scanner_data_Moving_Towards_Docking_WITH_Marker.txt',0,0); + +index_Moving = Scanner_Matrix_Moving(:,1); +scanner_value_Moving = Scanner_Matrix_Moving(:,2); + +index_Moving_Final = Scanner_Matrix_Moving(size(Scanner_Matrix_Moving,1),1); +scanner_value_Moving_Final = Scanner_Matrix_Moving(size(Scanner_Matrix_Moving,1),2); + +% figure; +% plot(index_Moving,scanner_value_Moving,'m',index_Moving_Final,scanner_value_Moving_Final,'k*'); +% legend('Distance to obstacle','Reference Position'); +% grid on; +% xlabel('Index'); +% ylabel('Distance to Obstacle [m]'); +% title('Scanner Analysis for a moving robot!'); +% axis([0 540 0 31]); + + +%% The robot is in the docking area, while NOT docked! +S_Mat_Out = csvread('scanner_data_outside.txt',0,0); +indx_out = S_Mat_Out(:,1); +s_val_out = S_Mat_Out(:,2); + +points_out = size(indx_out,1); +laser_beam_out = points_out - 1; +s_angle_out = zeros(points_out,1); +X_out = zeros(points_out,1); Y_out = zeros(points_out,1); +X_obst_out = zeros(points_out,1); Y_obst_out = zeros(points_out,1); + +for i = 1:points_out + s_angle_out(i) = (lower_angle + (indx_out(i)* ((upper_angle - lower_angle)/(laser_beam_out))))* pi/180; + X_out(i) = s_val_out(i) * cos(s_angle_out(i)); + Y_out(i) = s_val_out(i) * sin(s_angle_out(i)); + + X_obst_out(i) = dist_to_obst * cos(s_angle_out(i)); + Y_obst_out(i)= dist_to_obst * sin(s_angle_out(i)); +end + +% figure; +% set(gcf,'color','white'); +% subplot(1,2,1); +% plot(s_angle_out,s_val_out,'b','LineWidth',2); +% hold on +% plot(s_angle_out,dist_to_obst,'r','LineWidth',10); +% xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',14); +% ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',14); +% grid on; +% % axis([-2.23 2.23 -1 10]); +% +% subplot(1,2,2) +% plot(X_out,Y_out,'b','LineWidth',2); +% hold on +% plot(X_obst_out,Y_obst_out,'r-','LineWidth',4) +% xlabel('$X$ [m] ','interpreter','latex','FontSize',14); +% ylabel('$Y$ [m]','interpreter','latex','FontSize',14); +% +% l2 = legend('${Obstacles}$', '${Safety Margin (r = 10 cm)}$'); +% set(l2,'interpreter','latex','FontSize',12); +% % legend('boxoff'); +% grid on; +% % axis([-1.2 5 -3.5 1.5]); + + + +% figure; +% plot(X_DockedwithMar,Y_DockedwithMar,'b'); +% hold on; +% plot(X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',.3); +% xlabel('${X}$ [m]','interpreter','latex','FontSize',14); +% % ylabel('$ Y [m] $','interpreter','latex','FontSize',14); +% % grid on +% title('Detected marker (zoomed area from middle plot), TOP VIEW'); +% axis([.04 .16 -.12 .12]); + + + diff --git a/Files_4_thesis/scanner_analysis.m b/Files_4_thesis/scanner_analysis.m index 025b70470ae2b206f7b73bff3ac4ac7beff576bc..b2f3199c3686a10d7051192ac295e6e49a4a5c28 100644 --- a/Files_4_thesis/scanner_analysis.m +++ b/Files_4_thesis/scanner_analysis.m @@ -78,7 +78,7 @@ title(''); xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',20); ylabel('${Distance}$ [m]','interpreter','latex','FontSize',20); axis([-.09 .42 .08 .2]); -set(gca,'box','off','fontsize',20); +% set(gca,'box','off','fontsize',20); % legend('Experiment 1','Experiment 2','Experiment 3','Safety Curve'); subplot(1,2,2); % plot(X_exp_1,Y_exp_1,'m--','LineWidth',.9); @@ -96,10 +96,10 @@ title(''); xlabel('${X}$ [m]','interpreter','latex','FontSize',20); ylabel('${Y}$ [m]','interpreter','latex','FontSize',20); axis([-.1 .16 -.18 .18]); -set(gca,'box','off','fontsize',20); +% set(gca,'box','off','fontsize',20); l = legend('Trial 1','Trial 2','Trial 3','Safety Curve','Orientation','horizontal'); -set(l,'interpreter','latex','FontSize',13); - +set(l,'box','off','interpreter','latex','FontSize',13); +% legend('box','off') %% docked robot with marker vs. docked robot without marker! S_Mat_DockedwithMar = csvread('scanner_data_Docked_WITH_Marker.txt',0,0); S_Mat_DockedwithNOmar = csvread('scanner_data_no_cylinder.txt',0,0); @@ -263,7 +263,7 @@ subplot(3,2,5); orig_coordinate_box = gca; % create axes plot(orig_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01); hold on -plot(orig_coordinate_box,s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5); +plot(orig_coordinate_box,s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',3); xlabel('$ {\theta_b} $ [rad]','interpreter','latex','FontSize',14); axis([-2.3 2.3 -.15 5]); set(orig_coordinate_box,'box','off','fontsize',20); diff --git a/Paper/ifacconf_office/Laser_Scanner_Table.docx b/Paper/ifacconf_office/Laser_Scanner_Table.docx index b404624af078ab524a54e11b79d2ccf3a4d4fb56..1c4ff4c68be4fcd487422e4762341bd433360adb 100644 Binary files a/Paper/ifacconf_office/Laser_Scanner_Table.docx and b/Paper/ifacconf_office/Laser_Scanner_Table.docx differ diff --git a/Paper/ifacconf_office/Paper.docx b/Paper/ifacconf_office/Paper.docx index b2a50e03f1e2ac3e82a519876cb6e3dae27b3878..a13b0a392904764faa9ec17f8518892bad8e1818 100644 Binary files a/Paper/ifacconf_office/Paper.docx and b/Paper/ifacconf_office/Paper.docx differ diff --git a/Paper/ifacconf_office/Paper.pdf b/Paper/ifacconf_office/Paper.pdf new file mode 100644 index 0000000000000000000000000000000000000000..741e57992371973f29b42a7f155523601cf99909 Binary files /dev/null and b/Paper/ifacconf_office/Paper.pdf differ