diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 9b38fce2f5d9fc8eea47071350214678d7284a34..49803d710d6eac6ce620825e01403a06f61e5cd6 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -42,8 +42,6 @@
#include<VisionControl.h>
-
-
using namespace cv;
using namespace aruco;
using namespace std;
@@ -96,30 +94,26 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
// ---- Ref. Values for Logitech Camera ---- //
-const double PID4Docking::RefPose[4] = {-.0957, -0.030321 /* Y_ref*/ , 0.219427 /* X_ref*/ , -0.635269 /* theta_ref*/};
+const double PID4Docking::RefPose[4] = {-.0957, -0.0311278 /* Y_ref*/ , 0.219607 /* X_ref*/ , -0.616442 /* theta_ref*/};
// ---------------- PID gains---------------- //
-double PID4Docking::Kp_y = .65; //.7
-double PID4Docking::Ki_y = .001;//.001
-double PID4Docking::Kd_y = .2; // .2
+double PID4Docking::Kp_y = .48; //.55
+double PID4Docking::Ki_y = 0 ;//.002
+double PID4Docking::Kd_y = 0; //.1
-double PID4Docking::Kp_theta = .15 * Kp_y; // .2 * KP_y
-double PID4Docking::Ki_theta = .6 * Ki_y; // .07 * Ki_y
-double PID4Docking::Kd_theta = .00005 * Kd_y; // .00005 * Kd_y
+double PID4Docking::Kp_theta = .2;// .34 * Kp_y; .//35 * Kp_y (u can't put that gain less than certain value since the left joint on the robot would hit the docking platform!)
+double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
+double PID4Docking::Kd_theta = 0; //* Kd_y; // .0008
// ---------------- PID gains---------------- //
-
double PID4Docking::TT_S,PID4Docking::TT_E;
// random pose initialized
const double PID4Docking::y_up = .3;
const double PID4Docking::y_dwn = -.1;
-
const double PID4Docking::theta_dwn = -.7 /*-RefPose[3]*/;
const double PID4Docking::theta_up = .7 /*RefPose[3]*/;
double PID4Docking::x_new,PID4Docking::y_new,PID4Docking::theta_new;
-
-
double PID4Docking::dock_started,PID4Docking::dock_finished,PID4Docking::docking_duration;
double PID4Docking::speed_reducer_X = 1;
@@ -127,17 +121,18 @@ double PID4Docking::speed_reducer_Y = 1;
double PID4Docking::speed_reducer_theta = 1;
// ------ offsets X, Y, theta for Docking ---------
-double PID4Docking::x_thresh_dock = .001;
-double PID4Docking::y_thresh_dock = .002;
-double PID4Docking::theta_thresh_dock = (CV_PI/180) * 1; // 1 deg.
+double PID4Docking::Pz_eps = .001;
+double PID4Docking::Py_eps = .002;
+double PID4Docking::A_eps = (CV_PI/180) * 5; // 1 deg.
-double PID4Docking::safety_margin_X = .135; // safety margin X axis in docking process : 13.5 cm
+double PID4Docking::safety_margin_X = .13; // safety margin X axis in docking process : 18 cm
// ------ offsets X, Y, theta for Undocking ---------
double PID4Docking::x_thresh_undock = .02;
double PID4Docking::y_thresh_undock = .015;
double PID4Docking::theta_thresh_undock = (CV_PI/180) * 3;
+
double PID4Docking::docking_counter = 1;
PID4Docking::PID4Docking()
@@ -219,6 +214,7 @@ void PID4Docking::imageCb(const sensor_msgs::ImageConstPtr& msg)
img = cv_ptr->image;
}
+
bool PID4Docking::readArguments ( int argc,char **argv )
{
if (argc<2) {
@@ -317,6 +313,8 @@ if (TheVideoCapturer.retrieve(TheInputImage))
found = false;
keepMoving = false;
ROS_INFO_STREAM("Marker is lost, successful docking trials : " << (docking_counter - 1) << "\n");
+ //RandomPose(x_new,y_new,theta_new);
+ //move2docking(-control_signalX, -control_signalY, control_signalYAW);
}
if (node_vis.ok() && found)
@@ -421,11 +419,13 @@ if (TheVideoCapturer.retrieve(TheInputImage))
key=cv::waitKey(30);
// If space is hit, don't render the image.
- if (key == ' ')
- {
- update_images = !update_images;
- }
+if (key == ' ')
+{
+ update_images = !update_images;
+}
+
ros::spinOnce();
+
TT_E = ros::Time::now().toSec();
ROS_INFO_STREAM(" visualization while loop duration = " << (TT_E - TT_S) <<" \n");
@@ -463,9 +463,9 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
{
ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM --------- \n ");
if (
- (abs(RefPose[2] - camPose[2]) <= x_thresh_dock) //&& // Z
- //(abs(RefPose[1] - camPose[1]) <= y_thresh_dock) && // Y
- //(abs(RefPose[3] - camPose[3]) <= theta_thresh_dock) // Yaw
+ (abs(RefPose[2] - camPose[2]) <= Pz_eps) //&& // Z
+ //(abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y
+ //(abs(RefPose[3] - camPose[3]) <= A_eps) // Yaw
)
{
dock_finished = ros::Time::now().toSec();
@@ -482,15 +482,19 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
// to make sure that y & theta are within the threshold...
} else if (abs(RefPose[2] - camPose[2]) <= safety_margin_X)
{
- ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m., Fixing Y or theta. \n ");
-
- if((abs(RefPose[1] - camPose[1]) > y_thresh_dock) || (abs(abs(RefPose[3]) - abs(camPose[3])) > theta_thresh_dock))
- {
- speed_reducer_Y = 1;
- speed_reducer_theta = 1;
+ if(
+ (abs(RefPose[1] - camPose[1]) > Py_eps) ||
+ (abs(abs(RefPose[3]) - abs(camPose[3])) > A_eps)
+ )
+ {
+ ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m., Fixing Y or theta. \n ");
+ speed_reducer_Y = 1;
+ speed_reducer_theta = 1;
Controller(RefPose[2], RefPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
-
- } else if((abs(RefPose[1] - camPose[1]) <= y_thresh_dock) && (abs(abs(RefPose[3]) - abs(camPose[3])) <= theta_thresh_dock))
+ } else if(
+ (abs(RefPose[1] - camPose[1]) <= Py_eps) &&
+ (abs(abs(RefPose[3]) - abs(camPose[3])) <= A_eps)
+ )
{
ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
speed_reducer_X = .08;
@@ -498,12 +502,13 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
}
}else
{
- Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
+ speed_reducer_X = 1;
+ Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
}
} else
{
ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ---------\n");
- Undocking(x_new,y_new,theta_new);
+ RandomPose(x_new,y_new,theta_new);
}
}
@@ -512,7 +517,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
ROS_INFO_STREAM("--------------------- Controller started ----------------------\n ");
// -----------------X--------------------- //
- if(abs(RefX - MarPoseX) > x_thresh_dock)
+ if(abs(RefX - MarPoseX) > Pz_eps)
{
/*// e(t) = setpoint - actual value;
curr_errorX = RefX - MarPoseX;
@@ -535,11 +540,11 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
}
// -----------------Y--------------------- //
- if((RefY - MarPoseY) < -y_thresh_dock || (RefY - MarPoseY) > y_thresh_dock)
+ if((RefY - MarPoseY) < -Py_eps || (RefY - MarPoseY) > Py_eps)
{
// e(t) = setpoint - actual value;
curr_errorY = RefY - MarPoseY;
-
+
// Integrated error
int_errorY += curr_errorY * dt;
/*
@@ -562,21 +567,20 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
control_signalY = p_termY + i_termY + d_termY;
// robot & marker coordinates conversion
- control_signalY = -speed_reducer_Y * control_signalY;
+ control_signalY = - speed_reducer_Y * control_signalY;
prev_errorY = curr_errorY;
- }else if ((RefY - MarPoseY) <= y_thresh_dock && (RefY - MarPoseY) >= -y_thresh_dock)
+ } else if ((RefY - MarPoseY) <= Py_eps && (RefY - MarPoseY) >= -Py_eps)
{
control_signalY = 0;
}
// -------------------YAW--------------------------//
- if(abs(abs(RefYAW) - abs(MarPoseYAW)) > theta_thresh_dock)
+ if(abs(abs(RefYAW) - abs(MarPoseYAW)) > A_eps)
{
// e(t) = setpoint - actual value;
- curr_errorYAW = RefYAW - MarPoseYAW;
-
+ curr_errorYAW = abs(RefYAW) - abs(MarPoseYAW);
// Integrated error
int_errorYAW += curr_errorYAW * dt;
@@ -592,18 +596,32 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
ROS_INFO_STREAM("p_theta = " << p_termYAW << ", i_theta = " << i_termYAW << " d_theta = " << d_termYAW << " \n");
// control signal
control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
- //control_signalYAW = - control_signalYAW;
+
+ if(MarPoseYAW > 0 && abs(RefYAW) >= abs(MarPoseYAW))
+ {
+ ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation > 0 => CCW rotation\n"); // correct
+ control_signalYAW = speed_reducer_theta * control_signalYAW;
+
+ } else if (MarPoseYAW > 0 && abs(RefYAW) < abs(MarPoseYAW))
+ {
+ ROS_INFO("abs(RefYAW) < abs(orientation) && orientation > 0 => CCW rotation\n"); // correct ?
+ control_signalYAW = - speed_reducer_theta * control_signalYAW;
+
+ } else if (MarPoseYAW < 0 && abs(RefYAW) >= abs(MarPoseYAW))
+ {
+ ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation < 0 => CW rotation \n"); // correct
+ control_signalYAW = - speed_reducer_theta * control_signalYAW;
- if(MarPoseYAW > 0)
+ } else if (MarPoseYAW < 0 && abs(RefYAW) < abs(MarPoseYAW))
{
- ROS_INFO("Marker current orientation > 0, CCW rotation. \n");
- control_signalYAW = - speed_reducer_theta * control_signalYAW;
+ ROS_INFO("abs(RefYAW) < abs(orientation) && orientation < 0 => CCW rotation \n"); // correct ?
+ control_signalYAW = - speed_reducer_theta * control_signalYAW;
} else
{
- ROS_INFO("Marker current orientation < 0, CW rotation. \n");
- control_signalYAW = speed_reducer_theta * control_signalYAW;
+ ROS_INFO("New Condition should be added! \n");
+ keepMoving = false;
}
-
+
// save the current error as the previous one
// for the next iteration.
prev_errorYAW = curr_errorYAW;
@@ -611,12 +629,13 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
{
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)
@@ -630,7 +649,7 @@ void PID4Docking::dock(double VelX, double VelY, double omegaZ)
commandPub.publish(msg);
- ROS_INFO_STREAM(" Robot speed : \n" << msg);
+ ROS_INFO_STREAM(" Current speed of robot: \n" << msg << ".\n");
}
void PID4Docking::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
@@ -680,7 +699,7 @@ void PID4Docking::GenerateRandomVal()
theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn; // will be used for Q_Learning
}
-void PID4Docking::Undocking(double X_rand, double Y_rand, double theta_rand)
+void PID4Docking::RandomPose(double X_rand, double Y_rand, double theta_rand)
{
ROS_INFO_STREAM(" Xr = " << X_rand << ", Yr = " << Y_rand << ", Thetar = " << theta_rand << " rad ~ " << theta_rand * (180/CV_PI) << " deg\n");
ROS_INFO_STREAM(" -------------------------------------------------------------- \n");
@@ -783,3 +802,4 @@ geometry_msgs::Twist msg_new;
commandPub.publish(msg_new);
}
+
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 94c0e546693547263372b475ddcffd33e0863a53..5a86b9a7fd917326d5d63532fc37592797a5101d 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -42,8 +42,6 @@
#include<VisionControl.h>
-
-
using namespace cv;
using namespace aruco;
using namespace std;
@@ -96,33 +94,27 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
// ---- Ref. Values for Logitech Camera ---- //
-const double PID4Docking::RefPose[4] = {-.0957, -0.0295876 /* Y_ref*/ , 0.218695 /* X_ref*/ , -0.637745 /* theta_ref*/};
+const double PID4Docking::RefPose[4] = {-.0957, -0.0311278 /* Y_ref*/ , 0.219607 /* X_ref*/ , -0.616442 /* theta_ref*/};
// ---------------- PID gains---------------- //
-double PID4Docking::Kp_y = .7; //.7
-double PID4Docking::Ki_y = .001;//.005
-double PID4Docking::Kd_y = .2;
+double PID4Docking::Kp_y = .48; //.55
+double PID4Docking::Ki_y = 0 ;//.002
+double PID4Docking::Kd_y = 0; //.1
-double PID4Docking::Kp_theta = .2 * Kp_y;
-double PID4Docking::Ki_theta = .6 * Ki_y; // .07 * Ki_y
-double PID4Docking::Kd_theta = .00005 * Kd_y;
+double PID4Docking::Kp_theta = .22;// .34 * Kp_y; .//35 * Kp_y (u can't put that gain less than certain value since the left joint on the robot would hit the docking platform!)
+double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
+double PID4Docking::Kd_theta = 0; //* Kd_y; // .0008
// ---------------- PID gains---------------- //
-
double PID4Docking::TT_S,PID4Docking::TT_E;
// random pose initialized
const double PID4Docking::y_up = .3;
const double PID4Docking::y_dwn = -.1;
-
const double PID4Docking::theta_dwn = -.7 /*-RefPose[3]*/;
const double PID4Docking::theta_up = .7 /*RefPose[3]*/;
double PID4Docking::x_new,PID4Docking::y_new,PID4Docking::theta_new;
-
-
double PID4Docking::dock_started,PID4Docking::dock_finished,PID4Docking::docking_duration;
-double PID4Docking::zeroMax = .0000000000000000001;
-double PID4Docking::zeroMin = -.0000000000000000001;
double PID4Docking::speed_reducer_X = 1;
double PID4Docking::speed_reducer_Y = 1;
@@ -131,9 +123,9 @@ double PID4Docking::speed_reducer_theta = 1;
// ------ offsets X, Y, theta for Docking ---------
double PID4Docking::Pz_eps = .001;
double PID4Docking::Py_eps = .002;
-double PID4Docking::A_eps = (CV_PI/180) * 1; // 1 deg.
+double PID4Docking::A_eps = (CV_PI/180) * 5; // 1 deg.
-double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
+double PID4Docking::safety_margin_X = .13; // safety margin X axis in docking process : 18 cm
// ------ offsets X, Y, theta for Undocking ---------
double PID4Docking::x_thresh_undock = .02;
@@ -222,6 +214,7 @@ void PID4Docking::imageCb(const sensor_msgs::ImageConstPtr& msg)
img = cv_ptr->image;
}
+
bool PID4Docking::readArguments ( int argc,char **argv )
{
if (argc<2) {
@@ -509,7 +502,8 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
}
}else
{
- Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
+ speed_reducer_X = 1;
+ Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
}
} else
{
@@ -550,7 +544,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
{
// e(t) = setpoint - actual value;
curr_errorY = RefY - MarPoseY;
-
+
// Integrated error
int_errorY += curr_errorY * dt;
/*
@@ -573,11 +567,11 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
control_signalY = p_termY + i_termY + d_termY;
// robot & marker coordinates conversion
- control_signalY = -speed_reducer_Y * control_signalY;
+ control_signalY = - speed_reducer_Y * control_signalY;
prev_errorY = curr_errorY;
- }else if ((RefY - MarPoseY) <= Py_eps && (RefY - MarPoseY) >= -Py_eps)
+ } else if ((RefY - MarPoseY) <= Py_eps && (RefY - MarPoseY) >= -Py_eps)
{
control_signalY = 0;
}
@@ -586,8 +580,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
if(abs(abs(RefYAW) - abs(MarPoseYAW)) > A_eps)
{
// e(t) = setpoint - actual value;
- curr_errorYAW = RefYAW - MarPoseYAW;
-
+ curr_errorYAW = abs(RefYAW) - abs(MarPoseYAW);
// Integrated error
int_errorYAW += curr_errorYAW * dt;
@@ -603,17 +596,32 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
ROS_INFO_STREAM("p_theta = " << p_termYAW << ", i_theta = " << i_termYAW << " d_theta = " << d_termYAW << " \n");
// control signal
control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
- //control_signalYAW = - control_signalYAW;
- if(MarPoseYAW > 0)
+
+ if(MarPoseYAW > 0 && abs(RefYAW) >= abs(MarPoseYAW))
{
- ROS_INFO("Marker current orientation > 0, CCW rotation. \n");
- control_signalYAW = - speed_reducer_theta * control_signalYAW;
+ ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation > 0 => CCW rotation\n"); // correct
+ control_signalYAW = speed_reducer_theta * control_signalYAW;
+
+ } else if (MarPoseYAW > 0 && abs(RefYAW) < abs(MarPoseYAW))
+ {
+ ROS_INFO("abs(RefYAW) < abs(orientation) && orientation > 0 => CCW rotation\n"); // correct ?
+ control_signalYAW = - speed_reducer_theta * control_signalYAW;
+
+ } else if (MarPoseYAW < 0 && abs(RefYAW) >= abs(MarPoseYAW))
+ {
+ ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation < 0 => CW rotation \n"); // correct
+ control_signalYAW = - speed_reducer_theta * control_signalYAW;
+
+ } else if (MarPoseYAW < 0 && abs(RefYAW) < abs(MarPoseYAW))
+ {
+ ROS_INFO("abs(RefYAW) < abs(orientation) && orientation < 0 => CCW rotation \n"); // correct ?
+ control_signalYAW = - speed_reducer_theta * control_signalYAW;
} else
{
- ROS_INFO("Marker current orientation < 0, CW rotation. \n");
- control_signalYAW = speed_reducer_theta * control_signalYAW;
+ ROS_INFO("New Condition should be added! \n");
+ keepMoving = false;
}
-
+
// save the current error as the previous one
// for the next iteration.
prev_errorYAW = curr_errorYAW;
@@ -794,3 +802,4 @@ geometry_msgs::Twist msg_new;
commandPub.publish(msg_new);
}
+