diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index c68a8d85351cfc8085ed174c7572ad23f04d4d99..777140c17152d18cb57a1b24778859037bdc94d7 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -48,7 +48,7 @@ using namespace std;
float PID4Docking::TheMarkerSize = .1; // Default marker size
-int PID4Docking::Thresh1_min = 25;
+int PID4Docking::Thresh1_min = 75;
int PID4Docking::Thresh2_min = 10;
int PID4Docking::Thresh1_max = 300;
@@ -92,14 +92,14 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
//const double PID4Docking::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ , 0.308857 /* X_ref*/ , 0.17 /* theta_ref*/};
// ---- Ref. Values for Logitech Camera ---- //
-const double PID4Docking::RefPose[4] = {-.0957, 0.01923 /* Y_ref*/ , 0.201 /* X_ref*/ , -0.029 /* theta_ref*/};
+const double PID4Docking::RefPose[4] = {-.0957, 0.0199 /* Y_ref*/ , 0.201 /* X_ref*/ , -0.0335 /* theta_ref*/};
// ---------------- PID gains---------------- //
double PID4Docking::Kp_y = .38; //.4
double PID4Docking::Ki_y = 0 ;// 0
double PID4Docking::Kd_y = 0.02; //.1
-double PID4Docking::Kp_theta = .18;// .08
+double PID4Docking::Kp_theta = .25;// .18
double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
double PID4Docking::Kd_theta = 0; //* Kd_y; // .0008
// ---------------- PID gains---------------- //
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 9fc4bba54281690bcdaacebacd76bbe219219511..c68a8d85351cfc8085ed174c7572ad23f04d4d99 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -48,7 +48,7 @@ using namespace std;
float PID4Docking::TheMarkerSize = .1; // Default marker size
-int PID4Docking::Thresh1_min = 10;
+int PID4Docking::Thresh1_min = 25;
int PID4Docking::Thresh2_min = 10;
int PID4Docking::Thresh1_max = 300;
@@ -92,14 +92,14 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
//const double PID4Docking::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ , 0.308857 /* X_ref*/ , 0.17 /* theta_ref*/};
// ---- Ref. Values for Logitech Camera ---- //
-const double PID4Docking::RefPose[4] = {-.0957, -0.004 /* Y_ref*/ , 0.6752 /* X_ref*/ , -0.036 /* theta_ref*/};
+const double PID4Docking::RefPose[4] = {-.0957, 0.01923 /* Y_ref*/ , 0.201 /* X_ref*/ , -0.029 /* theta_ref*/};
// ---------------- PID gains---------------- //
-double PID4Docking::Kp_y = .51; //.4
+double PID4Docking::Kp_y = .38; //.4
double PID4Docking::Ki_y = 0 ;// 0
-double PID4Docking::Kd_y = 0.1; //.15
+double PID4Docking::Kd_y = 0.02; //.1
-double PID4Docking::Kp_theta = .11;// .08
+double PID4Docking::Kp_theta = .18;// .08
double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
double PID4Docking::Kd_theta = 0; //* Kd_y; // .0008
// ---------------- PID gains---------------- //
@@ -124,7 +124,7 @@ double PID4Docking::x_dock_thresh = .001;
double PID4Docking::y_dock_thresh = .002; //.002
double PID4Docking::theta_dock_thresh = (CV_PI/180) * .5; // .5 deg.
-double PID4Docking::safety_margin_X = .17; // safety margin X axis in docking process : 18 cm
+double PID4Docking::safety_margin_X = .16; // safety margin X axis in docking process : 18 cm
// ------ offsets X, Y, theta for Undocking ---------
double PID4Docking::x_thresh_undock = .02;
@@ -462,8 +462,7 @@ camPose[5] = CamFB->pose.orientation.z; // Robot yaw
ROS_INFO_STREAM(" X_mar = " << x_robINmar_coor << " vs. X_ref = " << RefPose[2] << " \n");
ROS_INFO_STREAM(" Y_mar = " << y_robINmar_coor << " vs. Y_ref = " << RefPose[1] << " \n");
- ROS_INFO_STREAM(" y_t = " << camPose[1] << " \n");
- ROS_INFO_STREAM(" x_t = " << camPose[2] << " \n");
+ ROS_INFO_STREAM(" y_t = " << camPose[1] << " , x_t = " << camPose[2] << "\n");
ROS_INFO_STREAM("------------------------------------------------------ \n ");
@@ -615,14 +614,14 @@ ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_term
control_signalYAW = 0;
}
- /* --- */
+ /* ---
ROS_INFO_STREAM("Control signalX = " << control_signalX <<"\n");
ROS_INFO_STREAM("Control signalY = " << control_signalY << "\n");
ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<"\n");
-
+ */
ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");
- //dock(control_signalX, control_signalY, control_signalYAW);
+ dock(control_signalX, control_signalY, control_signalYAW);
}
void PID4Docking::dock(double VelX, double VelY, double omegaZ)
@@ -636,7 +635,7 @@ void PID4Docking::dock(double VelX, double VelY, double omegaZ)
commandPub.publish(msg);
- ROS_INFO_STREAM(" Current speed of robot: \n" << msg << ".\n");
+ ROS_INFO_STREAM(" Current speed of robot: \n" << msg << "");
}
void PID4Docking::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
diff --git a/MobileRobot/docking_data/Pose.txt b/MobileRobot/docking_data/Pose_negative_angle.txt
similarity index 100%
rename from MobileRobot/docking_data/Pose.txt
rename to MobileRobot/docking_data/Pose_negative_angle.txt
diff --git a/MobileRobot/docking_data/Pose3.txt b/MobileRobot/docking_data/Pose_positive_angle.txt
similarity index 100%
rename from MobileRobot/docking_data/Pose3.txt
rename to MobileRobot/docking_data/Pose_positive_angle.txt
diff --git a/MobileRobot/docking_data/Pose2.txt b/MobileRobot/docking_data/Pose_zero_angle.txt
similarity index 100%
rename from MobileRobot/docking_data/Pose2.txt
rename to MobileRobot/docking_data/Pose_zero_angle.txt
diff --git a/MobileRobot/docking_data/Vel.txt b/MobileRobot/docking_data/Velocity_negative_angle.txt
similarity index 100%
rename from MobileRobot/docking_data/Vel.txt
rename to MobileRobot/docking_data/Velocity_negative_angle.txt
diff --git a/MobileRobot/docking_data/Vel3.txt b/MobileRobot/docking_data/Velocity_positive_angle.txt
similarity index 100%
rename from MobileRobot/docking_data/Vel3.txt
rename to MobileRobot/docking_data/Velocity_positive_angle.txt
diff --git a/MobileRobot/docking_data/Vel2.txt b/MobileRobot/docking_data/Velocity_zero_angle.txt
similarity index 100%
rename from MobileRobot/docking_data/Vel2.txt
rename to MobileRobot/docking_data/Velocity_zero_angle.txt
diff --git a/MobileRobot/docking_data/try.bag b/MobileRobot/docking_data/try.bag
deleted file mode 100644
index fe0167e5a62b178f12c75dada59bc392d6661c8c..0000000000000000000000000000000000000000
Binary files a/MobileRobot/docking_data/try.bag and /dev/null differ
diff --git a/MobileRobot/docking_data/try2.bag b/MobileRobot/docking_data/try2.bag
deleted file mode 100644
index 689501ec5848e4e5d26624fcadb9e529a13b9d6c..0000000000000000000000000000000000000000
Binary files a/MobileRobot/docking_data/try2.bag and /dev/null differ
diff --git a/MobileRobot/docking_data/try3.bag b/MobileRobot/docking_data/try3.bag
deleted file mode 100644
index 6d8407396e27b7dbc23c177d4c89e64f354e4345..0000000000000000000000000000000000000000
Binary files a/MobileRobot/docking_data/try3.bag and /dev/null differ