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
e1e3f635
Commit
e1e3f635
authored
Nov 29, 2016
by
Farid Alijani
Browse files
Paper mmodified
parent
8ed2a64d
Changes
12
Hide whitespace changes
Inline
Side-by-side
Files_4_thesis/Different_angle/docking_different_angle.m
View file @
e1e3f635
...
...
@@ -157,7 +157,7 @@ plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
% plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.6);
ylabel
(
'${x_{mar}}$ [m]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
title
(
'a) ${x_{mar}}$'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
set
(
gca
,
'fontsize'
,
20
);
%
set(gca,'fontsize',20);
subplot
(
323
);
plot
(
t_P_sec_data_1
,
Pose_Y_data_1
,
'LineWidth'
,
3
);
...
...
@@ -167,7 +167,7 @@ plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
% plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6);
ylabel
(
'${y_{mar}}$ [m]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
title
(
'b) ${y_{mar}}$'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
set
(
gca
,
'fontsize'
,
20
);
%
set(gca,'fontsize',20);
subplot
(
325
);
plot
(
t_P_sec_data_1
,
Theta_data_1
,
'LineWidth'
,
3
);
...
...
@@ -179,7 +179,7 @@ plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
title
(
'c) ${\theta_{mar}}$'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
ylabel
(
'${\theta_{mar}}$ [rad]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
xlabel
(
'$ time$ [s]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
set
(
gca
,
'fontsize'
,
20
);
%
set(gca,'fontsize',20);
% % control signals
% figure;
...
...
@@ -200,7 +200,7 @@ l2 = legend('${Left}$', '${Right}$');
set
(
l2
,
'interpreter'
,
'latex'
,
'FontSize'
,
14
);
legend
(
'boxoff'
)
axis
([
0
40
-.
01
.
18
]);
set
(
gca
,
'fontsize'
,
20
);
%
set(gca,'fontsize',20);
subplot
(
324
);
plot
(
t_V_sec_data_1
,
Vel_Y_data_1
,
'LineWidth'
,
3
);
...
...
@@ -210,7 +210,7 @@ plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
% plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.6);
ylabel
(
'$\dot{y}_{Rob}$ [m/s]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
title
(
'e) $\dot{y}_{Rob}$'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
set
(
gca
,
'fontsize'
,
20
);
%
set(gca,'fontsize',20);
subplot
(
326
);
plot
(
t_V_sec_data_1
,
Omega_Z_data_1
,
'LineWidth'
,
3
);
...
...
@@ -221,7 +221,7 @@ plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
xlabel
(
'$ time$ [s]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
ylabel
(
'$\dot{\theta}_{Rob}$ [rad/s]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
title
(
'f) $\dot{\theta}_{Rob}$'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
set
(
gca
,
'fontsize'
,
20
);
%
set(gca,'fontsize',20);
% Trajectory
...
...
@@ -247,9 +247,9 @@ hold on
plot
(
y_circle
,
x_circle
,
'k--'
,
'LineWidth'
,
3.4
);
% axis([-.4 .4 .18 1.4]);
title
(
'a) Approach
zone
+ SM
zone
+ Target
area
'
,
'FontSize'
,
17
);
title
(
'
${
a) Approach + SM + Target
}$'
,
'interpreter'
,
'latex
'
,
'FontSize'
,
20
);
ylabel
(
'${x}_{mar}$ [m]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
set
(
gca
,
'fontsize'
,
20
);
%
set(gca,'fontsize',20);
subplot
(
132
);
plot
(
Pose_Y_data_1
,
Pose_X_data_1
,
'b'
,
'LineWidth'
,
1.6
);
...
...
@@ -260,10 +260,10 @@ plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',1.6);
hold
on
plot
(
y_circle
,
x_circle
,
'k--'
,
'LineWidth'
,
3.4
);
title
(
'b) SM
zone
+ Target
area
'
,
'FontSize'
,
17
);
title
(
'
$ {
b) SM + Target
}$'
,
'interpreter'
,
'latex
'
,
'FontSize'
,
20
);
xlabel
(
'${y}_{mar}$ [m]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
axis
([
-.
03
0.03
-.
29
-.
191
]);
set
(
gca
,
'fontsize'
,
20
);
%
set(gca,'fontsize',20);
subplot
(
133
);
plot
(
Pose_Y_data_1
,
Pose_X_data_1
,
'b'
,
'LineWidth'
,
2
);
...
...
@@ -274,9 +274,9 @@ plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold
on
plot
(
y_circle
,
x_circle
,
'k--'
,
'LineWidth'
,
3.4
);
title
(
'c) Target
area
'
,
'FontSize'
,
17
);
title
(
'
$ {
c) Target
}$'
,
'interpreter'
,
'latex
'
,
'FontSize'
,
20
);
axis
([
-.
0202
-.
017
-.
2054
-.
199
]);
set
(
gca
,
'fontsize'
,
20
);
%
set(gca,'fontsize',20);
l
=
legend
(
'${Left}$'
,
'${Right}$'
,
'Target'
);
set
(
l
,
'interpreter'
,
'latex'
,
'FontSize'
,
14
);
...
...
Files_4_thesis/Different_controllers/docking_with_PID_controller.m
View file @
e1e3f635
...
...
@@ -612,9 +612,9 @@ xlabel('Sample number','interpreter','latex','FontSize',20);
% 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
);
%
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'
,
1
5
);
set
(
leg
,
'FontSize'
,
1
7
);
legend
(
'boxoff'
);
%
...
...
Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv
0 → 100644
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/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m
View file @
e1e3f635
...
...
@@ -22,12 +22,12 @@ clear all;
% 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
(
'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);
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...
...
...
@@ -52,13 +52,13 @@ t_V_sec_data_1(:,size(t_V_sec_data_1,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('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);
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...
...
...
@@ -80,12 +80,12 @@ 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('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);
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...
...
...
@@ -222,7 +222,7 @@ Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
figure
;
set
(
gcf
,
'color'
,
'white'
);
subplot
(
3
,
1
,
1
);
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
);
...
...
@@ -234,12 +234,12 @@ title('a) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
% xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
ylabel
(
'$\dot{x}_{Rob}$ [m/s]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
% grid on
%
axis([0 35 -.01 .
2
]);
axis
([
0
35
-.
01
.
18
]);
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
);
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
);
...
...
@@ -250,10 +250,10 @@ ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
% grid on
% axis([0 46 -.2 .1]);
subplot
(
3
,
1
,
3
);
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
);
plot
(
t_V_sec_data_2
,
Omega_Z_data_2
,
'r'
,
'LineWidth'
,
1.
8
);
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
);
...
...
@@ -304,16 +304,16 @@ 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
'
,
'FontSize'
,
17
);
title
(
'
${
a) Approach + SM + Target
}$'
,
'interpreter'
,
'latex
'
,
'FontSize'
,
20
);
% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
ylabel
(
'${x}_{mar}$ [m]'
,
'interpreter'
,
'latex'
,
'FontSize'
,
20
);
%
axis([-.65 .0
1
-1.47 -.1
85
]);
axis
([
-.
65
.
0
3
-
1.47
-.
1
6
]);
% 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'
,
1
4
);
set
(
l2
,
'interpreter'
,
'latex'
,
'FontSize'
,
1
5
);
legend
(
'boxoff'
);