Skip to content
GitLab
Menu
Projects
Groups
Snippets
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
farid
thesis
Commits
d3e0821b
Commit
d3e0821b
authored
Dec 01, 2016
by
Farid Alijani
Browse files
Paper modified
parent
e1e3f635
Changes
6
Hide whitespace changes
Inline
Side-by-side
Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv
deleted
100644 → 0
View file @
e1e3f635
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}$','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',18);
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('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]);
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('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;
% 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('${a) Approach zone + SM zone + Target area}$','interpreter','latex','FontSize',17);
% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
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');
set(l2,'interpreter','latex','FontSize',15);
legend('boxoff');
subplot(132);
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('$ {b) SM zone + Target area}$','interpreter','latex','FontSize',17);
xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
% 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('$ {c) Target area}$','interpreter','latex','FontSize',17);
% 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
Files_4_thesis/scanner_analysis.asv
deleted
100644 → 0
View file @
e1e3f635
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','vertical');
set(l,'interpreter','latex','FontSize',16);
% 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);
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);
set(gca,'box','off');
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);
set(gca,'box','off');
subplot(3,2,3);
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);
axis([-2.3 2.3 -.5 10]);
% set(orig_coordinate_cir,'box','off','fontsize',20);
set(orig_coordinate_cir,'box','off');
% ----- 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',10,...
'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,'b','LineWidth',2.7);
hold on;
plot(zoomed_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'r-','LineWidth',2.7);
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,'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 ------
