Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
farid
thesis
Commits
b8c10bfc
Commit
b8c10bfc
authored
Nov 22, 2016
by
Farid Alijani
Browse files
Report and paper and plots
parent
3dea1dee
Changes
11
Expand all
Hide whitespace changes
Inline
Side-by-side
Files_4_thesis/Different_controllers/docking_with_PID_controller.asv
0 → 100644
View file @
b8c10bfc
This diff is collapsed.
Click to expand it.
Files_4_thesis/Different_controllers/docking_with_PID_controller.m
View file @
b8c10bfc
...
...
@@ -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}} [s
ec
]$','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
)
...
...
Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv
0 → 100644
View file @
b8c10bfc
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
Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m
View file @
b8c10bfc
...
...
@@ -176,45 +176,45 @@ Omega_Z_data_3 = Vel_Matrix_data_3(:,7);