Skip to content
GitLab
Commit 32686fad authored by Farid Alijani's avatar Farid Alijani
Browse files

Merge branch 'master' of https://gitlab.control.lth.se/farid/thesis

parents 5f68aebb d16ed462
Expand all Hide whitespace changes
Inline Side-by-side
Files_4_thesis/Different_angle/docking_different_angle.m
View file @ 32686fad
...@@ -82,7 +82,7 @@ t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = []; ...@@ -82,7 +82,7 @@ t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];
% when using marker pose ,,,, % when using marker pose ,,,,
Pose_X_data_1 = Pose_Matrix_data_1(:,4); Pose_X_data_1 = Pose_Matrix_data_1(:,4);
Pose_Y_data_1 = Pose_Matrix_data_1(:,3); Pose_Y_data_1 = Pose_Matrix_data_1(:,3);
Theta_data_1 = Pose_Matrix_data_1(:,5); Theta_data_1 = Pose_Matrix_data_1(:,7);
% Extracting reference values when the robot is manually docked! % Extracting reference values when the robot is manually docked!
...@@ -95,7 +95,7 @@ ref_Pose_data_1 =[ref_X_data_1;ref_Y_data_1]; ...@@ -95,7 +95,7 @@ ref_Pose_data_1 =[ref_X_data_1;ref_Y_data_1];
% when using marker pose ,,,, % when using marker pose ,,,,
Pose_X_data_2 = Pose_Matrix_data_2(:,4); Pose_X_data_2 = Pose_Matrix_data_2(:,4);
Pose_Y_data_2 = Pose_Matrix_data_2(:,3); Pose_Y_data_2 = Pose_Matrix_data_2(:,3);
Theta_data_2 = Pose_Matrix_data_2(:,5); Theta_data_2 = Pose_Matrix_data_2(:,7);
% Extracting reference values when the robot is manually docked! % Extracting reference values when the robot is manually docked!
...@@ -108,7 +108,7 @@ ref_Pose_data_2 =[ref_X_data_2;ref_Y_data_2]; ...@@ -108,7 +108,7 @@ ref_Pose_data_2 =[ref_X_data_2;ref_Y_data_2];
% when using marker pose ,,,, % when using marker pose ,,,,
Pose_X_data_3 = Pose_Matrix_data_3(:,4); Pose_X_data_3 = Pose_Matrix_data_3(:,4);
Pose_Y_data_3 = Pose_Matrix_data_3(:,3); Pose_Y_data_3 = Pose_Matrix_data_3(:,3);
Theta_data_3 = Pose_Matrix_data_3(:,5); Theta_data_3 = Pose_Matrix_data_3(:,7);
% Extracting reference values when the robot is manually docked! % Extracting reference values when the robot is manually docked!
ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1)); ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1));
...@@ -140,7 +140,7 @@ Omega_Z_data_2 = Vel_Matrix_data_2(:,7); ...@@ -140,7 +140,7 @@ Omega_Z_data_2 = Vel_Matrix_data_2(:,7);
% data 3: % data 3:
Vel_X_data_3 = Vel_Matrix_data_3(:,2); Vel_X_data_3 = Vel_Matrix_data_3(:,2);
Vel_Y_data_3 = Vel_Matrix_data_3(:,3); Vel_Y_data_3 = abs(Vel_Matrix_data_3(:,3));
Omega_Z_data_3 = Vel_Matrix_data_3(:,7); Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
%% Plots %% Plots
...@@ -156,29 +156,29 @@ plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7); ...@@ -156,29 +156,29 @@ plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
% hold on % hold on
% plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.6); % plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.6);
title('Pose estimation in marker coordinate system'); title('Camera Pose Estimation');
ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14); ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14);
l2 = legend('Left', 'Right'); l2 = legend('Left', 'Right');
set(l2,'interpreter','latex','FontSize',11); set(l2,'interpreter','latex','FontSize',11);
legend('boxoff'); % legend('boxoff');
subplot(3,1,2); subplot(3,1,2);
plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',3); plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',3);
hold on hold on
plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7); plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
% hold on hold on
% plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6); plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6);
ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14); ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
subplot(3,1,3); subplot(3,1,3);
plot(t_P_sec_data_1,Theta_data_1,'LineWidth',3); plot(t_P_sec_data_1,Theta_data_1,'LineWidth',3);
hold on hold on
plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7); plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
% hold on hold on
% plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.6); plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.6);
xlabel('$ time [sec]$','interpreter','latex','FontSize',14); xlabel('$ time [s]$','interpreter','latex','FontSize',14);
ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14); ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);
% control signals % control signals
...@@ -190,8 +190,8 @@ subplot(311); ...@@ -190,8 +190,8 @@ subplot(311);
plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',3); plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',3);
hold on hold on
plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.7); plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.7);
% hold on hold on
% plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.6); plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.6);
title('Control Signals'); title('Control Signals');
ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14); ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
...@@ -212,9 +212,9 @@ subplot(3,1,3); ...@@ -212,9 +212,9 @@ subplot(3,1,3);
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',3); plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',3);
hold on hold on
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7); plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
% hold on hold on
% plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.6); plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.6);
xlabel('$ time [sec]$','interpreter','latex','FontSize',14); xlabel('$ time [s]$','interpreter','latex','FontSize',14);
ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14); ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
% Trajectory % Trajectory
...@@ -231,9 +231,9 @@ hold on ...@@ -231,9 +231,9 @@ hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2); plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
% hold on % hold on
% quiver(Pose_Y_data_2,Pose_X_data_2,gradient(Pose_Y_data_2),gradient(Pose_X_data_2),'m--','LineWidth',.01); % quiver(Pose_Y_data_2,Pose_X_data_2,gradient(Pose_Y_data_2),gradient(Pose_X_data_2),'m--','LineWidth',.01);
%
% hold on hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2); plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
% hold on % hold on
% quiver(Pose_Y_data_3,Pose_X_data_3,gradient(Pose_Y_data_3),gradient(Pose_X_data_3),'k--','LineWidth',.01); % quiver(Pose_Y_data_3,Pose_X_data_3,gradient(Pose_Y_data_3),gradient(Pose_X_data_3),'k--','LineWidth',.01);
...@@ -248,8 +248,8 @@ subplot(1,3,2); ...@@ -248,8 +248,8 @@ subplot(1,3,2);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',1.6); plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',1.6);
hold on hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',1.6); plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',1.6);
% hold on hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',1.6); plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',1.6);
hold on hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4); plot(y_circle,x_circle,'k--','LineWidth',3.4);
......
Files_4_thesis/Finding_X_dot_with_Pcontroller/CAM_x_dot_.1.txt 0 → 100644
View file @ 32686fad
This diff is collapsed.
Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m
View file @ 32686fad
...@@ -23,8 +23,11 @@ clear all; ...@@ -23,8 +23,11 @@ clear all;
% theta-axis gains: % theta-axis gains:
% P = .08; I = 0; D = 0; % P = .08; I = 0; D = 0;
Pose_Matrix_data_1 = csvread('Pose01_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); % 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 % time in position
TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec... TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
...@@ -45,16 +48,17 @@ t_V_sec_data_1 = 0:durationV_data_1/size(TimeV_ros_data_1,1):durationV_data_1; ...@@ -45,16 +48,17 @@ 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)) = []; t_V_sec_data_1(:,size(t_V_sec_data_1,2)) = [];
% ----------------------------------------------------------------------------------------------------- % -----------------------------------------------------------------------------------------------------
% data 2: 588 ros messages for geometry_msgs/Twist % data 2:
% 589 ros messages for geometry_msgs/PoseStamped % py = .25; ptheta = .25;
% y-axis gains: % iy = 0; itheta = 0;
% P = .5; I = 0.001; D = 0.03; % dy = 0; dtheta = 0;
% theta-axis gains: % Pose_Matrix_data_2 = csvread('Pose01_08_16_1749.txt',1,0);
% P = .28; I = 0.1; D = 0.15; % 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);
% time in position % time in position
TimeP_ros_data_2 = Pose_Matrix_data_2(:,1); % ros time, needs to be converted to sec... TimeP_ros_data_2 = Pose_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
...@@ -75,16 +79,13 @@ t_V_sec_data_2 = 0:durationV_data_2/size(TimeV_ros_data_2,1):durationV_data_2; ...@@ -75,16 +79,13 @@ 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)) = []; t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = [];
% ----------------------------------------------------------------------------------------------------- % -----------------------------------------------------------------------------------------------------
% data 3: 538 ros messages for geometry_msgs/Twist % data 3:
% 539 ros messages for geometry_msgs/PoseStamped % Pose_Matrix_data_3 = csvread('Pose01_08_16_1831.txt',1,0);
% y-axis gains: % Vel_Matrix_data_3 = csvread('Velocity01_08_16_1831.txt',1,0);
% P = .44; I = .0005; D = 0.15;
% theta-axis gains:
% P = .08; I = 0; D = 0;
Pose_Matrix_data_3 = csvread('Pose01_08_16_1831.txt',1,0); Pose_Matrix_data_3 = csvread('CAM_x_dot_.16.txt',1,0);
Vel_Matrix_data_3 = csvread('Velocity01_08_16_1831.txt',1,0); Vel_Matrix_data_3 = csvread('x_dot_.16.txt',1,0);
% time in position % time in position
TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec... TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
...@@ -111,13 +112,8 @@ t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = []; ...@@ -111,13 +112,8 @@ t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];
% when using marker pose ,,,, % when using marker pose ,,,,
Pose_X_data_1 = Pose_Matrix_data_1(:,4); Pose_X_data_1 = Pose_Matrix_data_1(:,4);
Pose_Y_data_1 = Pose_Matrix_data_1(:,3); Pose_Y_data_1 = Pose_Matrix_data_1(:,3);
Theta_data_1 = Pose_Matrix_data_1(:,5); Theta_data_1 = Pose_Matrix_data_1(:,7);
% when using robot odometry ,,,,
% Pose_X = Pose_Matrix(:,2);
% Pose_Y = Pose_Matrix(:,3);
% Theta = Pose_Matrix(:,7);
% Extracting reference values when the robot is manually docked! % Extracting reference values when the robot is manually docked!
ref_X_data_1 = Pose_X_data_1(size(Pose_X_data_1,1)); ref_X_data_1 = Pose_X_data_1(size(Pose_X_data_1,1));
...@@ -129,12 +125,7 @@ ref_Pose_data_1 =[ref_X_data_1;ref_Y_data_1]; ...@@ -129,12 +125,7 @@ ref_Pose_data_1 =[ref_X_data_1;ref_Y_data_1];
% when using marker pose ,,,, % when using marker pose ,,,,
Pose_X_data_2 = Pose_Matrix_data_2(:,4); Pose_X_data_2 = Pose_Matrix_data_2(:,4);
Pose_Y_data_2 = Pose_Matrix_data_2(:,3); Pose_Y_data_2 = Pose_Matrix_data_2(:,3);
Theta_data_2 = Pose_Matrix_data_2(:,5); Theta_data_2 = Pose_Matrix_data_2(:,7);
% when using robot odometry ,,,,
% Pose_X = Pose_Matrix(:,2);
% Pose_Y = Pose_Matrix(:,3);
% Theta = Pose_Matrix(:,7);
% Extracting reference values when the robot is manually docked! % Extracting reference values when the robot is manually docked!
ref_X_data_2 = Pose_X_data_2(size(Pose_X_data_2,1)); ref_X_data_2 = Pose_X_data_2(size(Pose_X_data_2,1));
...@@ -146,12 +137,8 @@ ref_Pose_data_2 =[ref_X_data_2;ref_Y_data_2]; ...@@ -146,12 +137,8 @@ ref_Pose_data_2 =[ref_X_data_2;ref_Y_data_2];
% when using marker pose ,,,, % when using marker pose ,,,,
Pose_X_data_3 = Pose_Matrix_data_3(:,4); Pose_X_data_3 = Pose_Matrix_data_3(:,4);
Pose_Y_data_3 = Pose_Matrix_data_3(:,3); Pose_Y_data_3 = Pose_Matrix_data_3(:,3);
Theta_data_3 = Pose_Matrix_data_3(:,5); Theta_data_3 = Pose_Matrix_data_3(:,7);
% when using robot odometry ,,,,
% Pose_X = Pose_Matrix(:,2);
% Pose_Y = Pose_Matrix(:,3);
% Theta = Pose_Matrix(:,7);
% Extracting reference values when the robot is manually docked! % Extracting reference values when the robot is manually docked!
ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1)); ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1));
...@@ -163,8 +150,8 @@ thresh_X = .001; ...@@ -163,8 +150,8 @@ thresh_X = .001;
theta = 0:.001:2*pi; theta = 0:.001:2*pi;
% needs to be adjusted manually if docking platform is replaced! % needs to be adjusted manually if docking platform is replaced!
ref_x = .2; ref_x = -.203;
ref_y = .0085; ref_y = -.019;
x_circle = thresh_X*cos(theta) + ref_x; % ref_X needs to be recorded 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 y_circle = thresh_X*sin(theta) + ref_y; % ref_Y needs to be recorded
...@@ -190,16 +177,6 @@ Omega_Z_data_3 = Vel_Matrix_data_3(:,7); ...@@ -190,16 +177,6 @@ Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
%% Plots %% Plots
% for the presenation all pose estimation(x,y,theta) in one plot
% figure;
% plot(t_P_sec,Pose_X,'b',t_P_sec,Pose_Y,'r',t_P_sec,Theta,'g');
%
% title('Marker Pose');
% xlabel('Time [sec]');
% ylabel('Pose');
% legend('X [m]','Y [m]','\theta [rad]');
% grid on
% marker position % marker position
figure; figure;
...@@ -235,7 +212,7 @@ hold on ...@@ -235,7 +212,7 @@ hold on
plot(t_P_sec_data_3,Theta_data_3,'g'); plot(t_P_sec_data_3,Theta_data_3,'g');
title('Marker \theta-axis'); title('Marker \theta-axis');
xlabel('Time [sec]'); xlabel('Time [s]');
ylabel('\theta [rad]'); ylabel('\theta [rad]');
grid on grid on
...@@ -253,11 +230,11 @@ hold on ...@@ -253,11 +230,11 @@ hold on
plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',2); plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',2);
title('Control Signals'); title('Control Signals');
xlabel('$ time [sec]$','interpreter','latex','FontSize',14); % 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',14);
% grid on % grid on
axis([0 60 -.01 .2]); axis([0 35 -.01 .2]);
l = legend('$\dot{x}_{Rob}$ = 0.15 [m/s]','$\dot{x}_{Rob}$ = 0.1[m/s]','$\dot{x}_{Rob}$ = 0.16[m/s]'); 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',12);
legend('boxoff'); legend('boxoff');
...@@ -281,13 +258,16 @@ hold on ...@@ -281,13 +258,16 @@ hold on
plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',2.1); plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',2.1);
% title('Control Signal \theta_{Rob} - axis'); % title('Control Signal \theta_{Rob} - axis');
xlabel('$ time [sec]$','interpreter','latex','FontSize',14); xlabel('$ time [s]$','interpreter','latex','FontSize',14);
ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14); ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
% grid on % grid on
% axis([0 46 -.1 .15]); % axis([0 46 -.1 .15]);
% Trajectory % Trajectory
% figure; % figure;
% plot(Pose_Y_data_1,Pose_X_data_1,'b',ref_Y_data_1,ref_X_data_1,'k*'); % plot(Pose_Y_data_1,Pose_X_data_1,'b',ref_Y_data_1,ref_X_data_1,'k*');
% hold on % hold on
...@@ -318,7 +298,7 @@ ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14); ...@@ -318,7 +298,7 @@ ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
figure; figure;
set(gcf,'color','white'); set(gcf,'color','white');
subplot(1,3,1); subplot(131);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2); plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2); plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
...@@ -328,12 +308,12 @@ hold on ...@@ -328,12 +308,12 @@ hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4) plot(y_circle,x_circle,'k--','LineWidth',3.4)
title('Approach zone + SM zone + Target area'); title('Approach zone + SM zone + Target area');
xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14); % xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',14); ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',16);
axis([-.3 .4 .18 1.4]); axis([-.65 .01 -1.47 -.185]);
% grid on % grid on
l2 = legend('$\dot{x}_{Rob}$ = 0.15[m/s]','$\dot{x}_{Rob}$ = 0.1[m/s]','$\dot{x}_{Rob}$ = 0.16[m/s]','Target','Location','northwest'); 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'); set(l2,'interpreter','latex','FontSize',14);
legend('boxoff'); legend('boxoff');
subplot(1,3,2); subplot(1,3,2);
...@@ -345,12 +325,12 @@ plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2); ...@@ -345,12 +325,12 @@ plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4); plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('SM zone + Target area'); title('SM zone + Target area');
xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14); xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',16);
% ylabel('${X}$ [m]','interpreter','latex'); % ylabel('${X}$ [m]','interpreter','latex');
axis([-.01 0.06 .195 .32]); axis([-.033 -.01 -.26 -.195]);
% grid on % grid on
subplot(1,3,3); subplot(133);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2); plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2); plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
...@@ -359,9 +339,9 @@ plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2); ...@@ -359,9 +339,9 @@ plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4); plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('Target area'); title('Target area');
xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14); % xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
% ylabel('${X}$ [m]','interpreter','latex'); % ylabel('${X}$ [m]','interpreter','latex');
axis([0.006 .011 .198 .204]); axis([-.0205 -.0175 -.206 -.20]);
% grid on % grid on
% l2 = legend('x_dot = 0.15','x_dot = 0.1','x_dot = 0.16','Reference Position'); % l2 = legend('x_dot = 0.15','x_dot = 0.1','x_dot = 0.16','Reference Position');
...@@ -396,26 +376,26 @@ ylabel('Sampling time'); ...@@ -396,26 +376,26 @@ ylabel('Sampling time');
% xlabel('samples'); % xlabel('samples');
% ylabel('time step'); % ylabel('time step');
% axis([150 250 0 .02]); % axis([150 250 0 .02]);
figure; % figure;
set(gcf,'color','white'); % set(gcf,'color','white');
%
subplot(3,1,1); % subplot(3,1,1);
plot(diff(Pose_X_data_1)); % plot(diff(Pose_X_data_1));
title('X-Pose diff'); % title('X-Pose diff');
xlabel('samples'); % xlabel('samples');
ylabel('X difference [m]'); % ylabel('X difference [m]');
grid on % grid on
%
subplot(3,1,2); % subplot(3,1,2);
plot(diff(Pose_X_data_1)); % plot(diff(Pose_X_data_1));
title('Y-Pose diff'); % title('Y-Pose diff');
xlabel('samples'); % xlabel('samples');
ylabel('Y difference [m]'); % ylabel('Y difference [m]');
grid on % grid on
%
subplot(3,1,3); % subplot(3,1,3);
plot(diff(Theta_data_1)); % plot(diff(Theta_data_1));
title('\theta-Pose diff'); % title('\theta-Pose diff');
xlabel('samples'); % xlabel('samples');
ylabel('\theta difference [rad]'); % ylabel('\theta difference [rad]');
grid on % grid on
\ No newline at end of file \ No newline at end of file
Files_4_thesis/Finding_X_dot_with_Pcontroller/x_dot_.1.txt 0 → 100644
View file @ 32686fad
This diff is collapsed.
Files_4_thesis/Finding_X_dot_with_Pcontroller/x_dot_.15.txt 0 → 100644
View file @ 32686fad
This diff is collapsed.
Files_4_thesis/Finding_X_dot_with_Pcontroller/x_dot_.16.txt 0 → 100644
View file @ 32686fad
This diff is collapsed.
Files_4_thesis/x_tANDy_t/y_tANDx_t.m
View file @ 32686fad
...@@ -198,12 +198,12 @@ hold on ...@@ -198,12 +198,12 @@ hold on
plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.8); plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.8);
hold on hold on
plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.3); plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.3);
title('Pose estimation in marker coordinate system'); title('Camera Pose Estimation');
ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14); ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14);
l2 = legend('${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$', '${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$'); l2 = legend('${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$', '${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$');
set(l2,'interpreter','latex','FontSize',11); set(l2,'interpreter','latex','FontSize',9);
legend('boxoff'); % legend('boxoff');
subplot(312); subplot(312);
plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',1.8); plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',1.8);
...@@ -224,7 +224,7 @@ plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.6); ...@@ -224,7 +224,7 @@ plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.6);
hold on hold on
plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.4); plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.4);
xlabel('$ time [sec]$','interpreter','latex','FontSize',14);