diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index d92b9ae7ee009fbf8c71a066d6abc8f022e53667..9619df65eeb66cca77437283f92615756fe56595 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -175,6 +175,7 @@ public:
static double Py_eps,Pz_eps,A_eps;
static double x_thresh_undock,y_thresh_undock,theta_thresh_undock;
static double rand_X_SM,rand_A_esp,rand_Y_esp;
+ static double speed_reducer;
double marpose[6];
double camPose[6];
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index 8fa1d913a4aed34afa7e7b2adbf24d4cde6f96b2..d92b9ae7ee009fbf8c71a066d6abc8f022e53667 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -127,6 +127,8 @@ public:
static double S_Ang_P,S_Ang_I,S_Ang_D;
static double L_Ang_P,L_Ang_I,L_Ang_D;
+ static double Ang_P,Ang_I,Ang_D;
+
static double x_new,y_new,theta_new;
static double safety_margin_X;
@@ -166,7 +168,7 @@ public:
static int msec,sec,min,hr;
// ---- TIMER PARAMETERS ------ //
-
+ static double docking_counter;
static double dock_started,dock_finished,docking_duration;
static double zeroMin,zeroMax;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 933df333ab0f9ae73fd04550d31cef12f83af6ad..18e3e50d7ff8d8bb5c5dc8b95f70694b9ce853cb 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -97,42 +97,31 @@ double ImageConverter::control_signalYAW;
// ---- CONTROLL PARAMETERS ------ //
// ---------------- Initial Gains which dock the robot successfully ---------------- //
+double ImageConverter::Pos_Py = .4;
+double ImageConverter::Pos_Iy = .0042;
+double ImageConverter::Pos_Dy = 0;
-double ImageConverter::Pos_Px = .1;
-double ImageConverter::Pos_Ix = 0.0028;
-double ImageConverter::Pos_Dx = 0;
-
-double ImageConverter::Pos_Py = 4 * Pos_Px;
-double ImageConverter::Pos_Iy = 1.5 * Pos_Ix;
-double ImageConverter::Pos_Dy = 4 * Pos_Dx;
-
-/*double ImageConverter::S_Ang_P = .2 * Pos_Px;
-double ImageConverter::S_Ang_I = .2 * Pos_Ix;
-double ImageConverter::S_Ang_D = .2 * Pos_Dx;
-
-double ImageConverter::L_Ang_P = .8 * Pos_Px;
-double ImageConverter::L_Ang_I = .8 * Pos_Ix;
-double ImageConverter::L_Ang_D = .8 * Pos_Dx;*/
-
-double ImageConverter::Ang_P = .8 * Pos_Px;
-double ImageConverter::Ang_I = .8 * Pos_Ix;
-double ImageConverter::Ang_D = .8 * Pos_Dx;
+double ImageConverter::Ang_P = .2 * Pos_Py;
+double ImageConverter::Ang_I = (8/15) * Pos_Iy;
+double ImageConverter::Ang_D = .2 * Pos_Dy;
double ImageConverter::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration;
double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
+double ImageConverter::speed_reducer = 1;
+
// ------ offsets X, Y, theta for Docking ---------
double ImageConverter::Pz_eps = .001;
double ImageConverter::Py_eps = .002;
double ImageConverter::A_eps = (CV_PI/180) * .5; // 1 deg.
-double ImageConverter::safety_margin_X = .1; // safety margin X axis in docking process : 10 cm
+double ImageConverter::safety_margin_X = .15; // safety margin X axis in docking process : 15 cm
// ------ offsets X, Y, theta for Undocking ---------
double ImageConverter::x_thresh_undock = .02;
-double ImageConverter::y_thresh_undock = .01;
+double ImageConverter::y_thresh_undock = .015;
double ImageConverter::theta_thresh_undock = (CV_PI/180) * 3;
// random pose initialized
@@ -151,8 +140,8 @@ double ImageConverter::docking_counter = 1;
// Ref. Values
RefPose[0] = -.0957;
-RefPose[1] = -0.0196804;
-RefPose[2] = 0.305155;
+RefPose[1] = -0.0194504;
+RefPose[2] = 0.304966;
RefPose[3] = 0.703702;
x_rand_SM = RefPose[2] + .55; // 55 cm spreading domain in the x-axis while moving towards the random pose
@@ -474,9 +463,6 @@ camPose[0] = CamFB->pose.position.x;
camPose[1] = CamFB->pose.position.y;
camPose[2] = CamFB->pose.position.z;
camPose[3] = CamFB->pose.orientation.x;
-
- //ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n");
- //ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n");
// in Marker coordinate sys.
@@ -487,11 +473,9 @@ camPose[3] = CamFB->pose.orientation.x;
// correspondingly ...
// roll in Marker coordinate => yaw in Robot coordinate
- /*ROS_INFO_STREAM(" --------------------- TIMER ------------------ \n");
- ROS_INFO_STREAM("Docking duration : "<< hr << " : " << min << " : " << sec << " . " << msec << "\n");*/
ROS_INFO_STREAM("--------- PID gains in trial no. " << docking_counter << " : ---------\n");
- ROS_INFO_STREAM(" Kp = " << Pos_Px << " , Ki = " << Pos_Ix << " , Kd = " << Pos_Dx << "\n");
+ ROS_INFO_STREAM(" Kp = " << Pos_Py << " , Ki = " << Pos_Iy << " , Kd = " << Pos_Dy << "\n");
ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs X_ref = " << RefPose[2] << " \n");
@@ -503,7 +487,7 @@ camPose[3] = CamFB->pose.orientation.x;
if(Go2RandomPose == false)
{
- ROS_INFO_STREAM("---------- MOVING TOWARDS THE DOCKING STATION ... --------- \n ");
+ ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM ... --------- \n ");
if (
(abs(RefPose[2] - camPose[2]) <= Pz_eps) //&& // Z
//(abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y
@@ -516,6 +500,7 @@ camPose[3] = CamFB->pose.orientation.x;
//keepMoving = false;
GenerateRandomVal();
docking_counter ++;
+ speed_reducer = 1;
Go2RandomPose = true;
// to make sure that y & theta are within the threshold...
@@ -534,6 +519,7 @@ camPose[3] = CamFB->pose.orientation.x;
)
{
ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
+ speed_reducer = .1;
Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.01);
}
}else
@@ -542,7 +528,7 @@ camPose[3] = CamFB->pose.orientation.x;
}
} else
{
- ROS_INFO(" Random pose : \n");
+ ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ... ---------\n");
RandomPose(x_new,y_new,theta_new);
}
}
@@ -550,41 +536,32 @@ camPose[3] = CamFB->pose.orientation.x;
void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
{
ROS_INFO_STREAM("--------------------- Controller started ----------------------\n ");
- // -----------------X--------------------- //
+ // -----------------X--------------------- //
if(abs(RefX - MarPoseX) > Pz_eps)
{
- // e(t) = setpoint - actual value;
+ /*// e(t) = setpoint - actual value;
curr_errorX = RefX - MarPoseX;
// Integrated error
int_errorX += curr_errorX * dt;
- /*
- // -- windup gaurd --
- if (int_error < )
- {}
- else if ()
- {}*/
-
// differentiation
diffX = ((curr_errorX - prev_errorX) / dt);
-
// scalling
p_termX = Pos_Px * curr_errorX;
i_termX = Pos_Ix * int_errorX;
d_termX = Pos_Dx * diffX;
-
// control signal
control_signalX = p_termX + i_termX + d_termX;
-
-
- // save the current error as the previous one
- // for the next iteration.
- prev_errorX = curr_errorX;
+ prev_errorX = curr_errorX;*/
+ control_signalX = speed_reducer * 0.05;
} else
{
control_signalX = 0;
}
+
+
+
// -----------------Y--------------------- //
if((RefY - MarPoseY) < -Py_eps || (RefY - MarPoseY) > Py_eps)
@@ -609,7 +586,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
i_termY = Pos_Iy * int_errorY;
d_termY = Pos_Dy * diffY;
- ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n");
+ //ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n");
// control signal
control_signalY = p_termY + i_termY + d_termY;
@@ -642,27 +619,6 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
// differentiation
diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
-
-
- /*// YAW offset...
- int yaw_offset = 15;
- if (curr_errorYAW < ((CV_PI/180) * yaw_offset) && curr_errorYAW > ((CV_PI/180) * -yaw_offset)) // -5 < err < +5
- {
- ROS_INFO_STREAM(" robot is INSIDE Reference boundry of " << yaw_offset << " deg. \n");
- // scalling
- p_termYAW = S_Ang_P * curr_errorYAW;
- i_termYAW = S_Ang_I * int_errorYAW;
- d_termYAW = S_Ang_D * diffYAW;
- //ROS_INFO_STREAM("prop_gainYAW = " << p_termYAW << ", integ_gainYAW = " << i_termYAW << " . \n");
- } else if (curr_errorYAW > ((CV_PI/180) * yaw_offset) || curr_errorYAW < ((CV_PI/180) * -yaw_offset)) // err > +5 or err < -5
- {
- ROS_INFO_STREAM(" robot is OUTSIDE Reference boundry of " << yaw_offset << " deg. \n");
- // scalling
- p_termYAW = L_Ang_P * curr_errorYAW;
- i_termYAW = L_Ang_I * int_errorYAW;
- d_termYAW = L_Ang_D * diffYAW;
- //ROS_INFO_STREAM("prop_gainYAW = " << p_termYAW << ", integ_gainYAW = " << i_termYAW << " . \n");
- }*/
ROS_INFO_STREAM(" adjusting orientation! \n");
// scalling
@@ -695,7 +651,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
//ROS_INFO_STREAM("RAW 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);
+ dock(control_signalX, control_signalY, control_signalYAW);
}
void ImageConverter::dock(double VelX, double VelY, double omegaZ)
@@ -747,9 +704,9 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega
void ImageConverter::GenerateRandomVal()
{
// ---------------- PID gains ------------------
- Pos_Px = ((double) rand() / (RAND_MAX)) * (.19 - .1) + .1; // .1 < Kp < .16
- Pos_Ix = ((double) rand() / (RAND_MAX)) * .01; // 0 < Ki < .01
- Pos_Dx = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .01
+ Pos_Py = ((double) rand() / (RAND_MAX)) * (.76 - .4) + .4; // .1 < Kp < .76
+ Pos_Iy = ((double) rand() / (RAND_MAX)) * .006; // 0 < Ki < .007
+ Pos_Dy = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .02
// ------------------ Generating Random Pose ------------------
x_new = ((double) rand() / (RAND_MAX)) * (1.1 - x_rand_SM) + x_rand_SM;
@@ -849,31 +806,12 @@ geometry_msgs::Twist msg_new;
vel_y = -.03;
}
- }/* else if ((Y_rand < 0) && (theta_rand < 0) && (camPose[1] - Y_rand > y_thresh_undock))
- { // extream case on the right side, move 2 right, CCW
-
- ROS_INFO(" EXTREAM case on right side, move 2 right, CCW \n");
- if ((abs(Y_rand) - camPose[1]) > y_thresh_undock)
- {
- vel_y = -.01;
- omega_z = .05;
- }
- } else if ((Y_rand > 0) && (theta_rand > 0) && (camPose[1] - Y_rand < -y_thresh_undock))
- { // extream case on the left side
-
- ROS_INFO(" EXTREAM case on left side, move 2 left, CW \n");
- if ((abs(Y_rand) - camPose[1]) > y_thresh_undock)
- {
- vel_y = .01;
- omega_z = -.05;
- }
- }*/ else
+ } else
{
ROS_INFO(" New condition should be added! \n");
keepMoving = false;
}
}
-
msg_new.linear.x = vel_x;
msg_new.linear.y = vel_y;
msg_new.angular.z = omega_z;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index c30473c2c114ddb863b300c90d5033a936f79d39..d8d96b0d715670d0e836a7e8f2706134ac9aa7cf 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -103,7 +103,7 @@ double ImageConverter::Pos_Ix = 0.0028;
double ImageConverter::Pos_Dx = 0;
double ImageConverter::Pos_Py = 4 * Pos_Px;
-double ImageConverter::Pos_Iy = .5 * Pos_Ix;
+double ImageConverter::Pos_Iy = 1.5 * Pos_Ix;
double ImageConverter::Pos_Dy = 4 * Pos_Dx;
/*double ImageConverter::S_Ang_P = .2 * Pos_Px;
@@ -125,13 +125,13 @@ double ImageConverter::zeroMin = -.0000000000000000001;
// ------ offsets X, Y, theta for Docking ---------
double ImageConverter::Pz_eps = .001;
-double ImageConverter::Py_eps = .0015;
+double ImageConverter::Py_eps = .002;
double ImageConverter::A_eps = (CV_PI/180) * .5; // 1 deg.
double ImageConverter::safety_margin_X = .1; // safety margin X axis in docking process : 10 cm
// ------ offsets X, Y, theta for Undocking ---------
-double ImageConverter::x_thresh_undock = .01;
+double ImageConverter::x_thresh_undock = .02;
double ImageConverter::y_thresh_undock = .01;
double ImageConverter::theta_thresh_undock = (CV_PI/180) * 3;
@@ -151,11 +151,11 @@ double ImageConverter::docking_counter = 1;
// Ref. Values
RefPose[0] = -.0957;
-RefPose[1] = -0.0196805;
-RefPose[2] = 0.306154;
-RefPose[3] = 0.702366;
+RefPose[1] = -0.0196804;
+RefPose[2] = 0.305155;
+RefPose[3] = 0.703702;
-x_rand_SM = RefPose[2] + .45; // 45 cm spreading domain in the x-axis while moving towards the random pose
+x_rand_SM = RefPose[2] + .55; // 55 cm spreading domain in the x-axis while moving towards the random pose
//marSub = nh_.subscribe("/Marker_pose",1,&ImageConverter::marCB,this);
@@ -320,7 +320,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
{
found = false;
keepMoving = false;
- ROS_INFO_STREAM("damn, marker is lost, but " << docking_counter<< " successful docking trials ... \n");
+ 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);
}
@@ -475,9 +475,6 @@ camPose[1] = CamFB->pose.position.y;
camPose[2] = CamFB->pose.position.z;
camPose[3] = CamFB->pose.orientation.x;
- //ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n");
- //ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n");
-
// in Marker coordinate sys.
// z => X robot (thrust)
@@ -487,20 +484,15 @@ camPose[3] = CamFB->pose.orientation.x;
// correspondingly ...
// roll in Marker coordinate => yaw in Robot coordinate
- /*ROS_INFO_STREAM(" --------------------- TIMER ------------------ \n");
- ROS_INFO_STREAM("Docking duration : "<< hr << " : " << min << " : " << sec << " . " << msec << "\n");*/
ROS_INFO_STREAM("--------- PID gains in trial no. " << docking_counter << " : ---------\n");
ROS_INFO_STREAM(" Kp = " << Pos_Px << " , Ki = " << Pos_Ix << " , Kd = " << Pos_Dx << "\n");
ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
- ROS_INFO_STREAM(" Xmar = " << camPose[2] << " m. \n");
- ROS_INFO_STREAM(" Xref = " << RefPose[2] << " m. \n");
-
- ROS_INFO_STREAM(" Ymar = " << camPose[1] << " m. \n");
- ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n");
+ ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs X_ref = " << RefPose[2] << " \n");
+ ROS_INFO_STREAM(" Y_mar = " << camPose[1] << " vs Y_ref = " << RefPose[1] << " \n");
- ROS_INFO_STREAM(" theta_rob = " << camPose[3] << " rad. =~ " << (180/CV_PI) * camPose[3] << " deg. \n");
+ ROS_INFO_STREAM(" theta_mar = " << camPose[3] << " rad. =~ " << (180/CV_PI) * camPose[3] << " deg. \n");
ROS_INFO_STREAM(" theta_ref = " << RefPose[3] << " rad. =~ " << (180/CV_PI) * RefPose[3] << " deg. \n");
ROS_INFO_STREAM("------------------------------------------------------ \n ");
@@ -512,11 +504,10 @@ camPose[3] = CamFB->pose.orientation.x;
//(abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y
//(abs(RefPose[3] - camPose[3]) <= A_eps) // Yaw
)
- {
- //ROS_INFO("---- ***** Robot is docked successfully, Moving 2 new Random Pose ***** ---- \n ");
+ {
dock_finished = ros::Time::now().toSec();
docking_duration = dock_finished - dock_started;
- ROS_INFO_STREAM("docking No. " << docking_counter << " is finished in "<< docking_duration <<" sec, Moving 2 new Random Pose\n");
+ ROS_INFO_STREAM("docking No. " << docking_counter << " is finished in "<< docking_duration <<" sec, moving to new Random Pose\n");
//keepMoving = false;
GenerateRandomVal();
docking_counter ++;
@@ -546,7 +537,7 @@ camPose[3] = CamFB->pose.orientation.x;
}
} else
{
- ROS_INFO(" moving towards Generated Random pose which is : \n");
+ ROS_INFO(" Random pose : \n");
RandomPose(x_new,y_new,theta_new);
}
}
@@ -630,7 +621,6 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
}
// -------------------YAW--------------------------//
-
if(abs(RefYAW - abs(MarPoseYAW)) > A_eps)
{
// e(t) = setpoint - actual value;
@@ -648,28 +638,6 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
// differentiation
diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
-
- /*// YAW offset...
- int yaw_offset = 15;
- if (curr_errorYAW < ((CV_PI/180) * yaw_offset) && curr_errorYAW > ((CV_PI/180) * -yaw_offset)) // -5 < err < +5
- {
- ROS_INFO_STREAM(" robot is INSIDE Reference boundry of " << yaw_offset << " deg. \n");
- // scalling
- p_termYAW = S_Ang_P * curr_errorYAW;
- i_termYAW = S_Ang_I * int_errorYAW;
- d_termYAW = S_Ang_D * diffYAW;
- //ROS_INFO_STREAM("prop_gainYAW = " << p_termYAW << ", integ_gainYAW = " << i_termYAW << " . \n");
- } else if (curr_errorYAW > ((CV_PI/180) * yaw_offset) || curr_errorYAW < ((CV_PI/180) * -yaw_offset)) // err > +5 or err < -5
- {
- ROS_INFO_STREAM(" robot is OUTSIDE Reference boundry of " << yaw_offset << " deg. \n");
- // scalling
- p_termYAW = L_Ang_P * curr_errorYAW;
- i_termYAW = L_Ang_I * int_errorYAW;
- d_termYAW = L_Ang_D * diffYAW;
- //ROS_INFO_STREAM("prop_gainYAW = " << p_termYAW << ", integ_gainYAW = " << i_termYAW << " . \n");
- }*/
-
-
ROS_INFO_STREAM(" adjusting orientation! \n");
// scalling
p_termYAW = Ang_P * curr_errorYAW;
@@ -702,7 +670,6 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");
dock(-control_signalX, control_signalY, control_signalYAW);
-
}
void ImageConverter::dock(double VelX, double VelY, double omegaZ)
@@ -759,16 +726,16 @@ void ImageConverter::GenerateRandomVal()
Pos_Dx = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .01
// ------------------ Generating Random Pose ------------------
- x_new = ((double) rand() / (RAND_MAX)) * (1.05 - x_rand_SM) + x_rand_SM;
- y_new = ((double) rand() / (RAND_MAX)) * (.5 - (-.24)) + (-.24);
+ x_new = ((double) rand() / (RAND_MAX)) * (1.1 - x_rand_SM) + x_rand_SM;
+ y_new = ((double) rand() / (RAND_MAX)) * (.48 - (-.24)) + (-.24);
theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]);
}
void ImageConverter::RandomPose(double X_rand, double Y_rand, double theta_rand)
{
- ROS_INFO_STREAM(" X_rand = " << X_rand << " m. \n");
- ROS_INFO_STREAM(" Y_rand = " << Y_rand << " m. \n");
- ROS_INFO_STREAM(" theta_rand = " << theta_rand << " rad. =~ " << theta_rand * (180/CV_PI) << " deg. \n");
+ ROS_INFO_STREAM(" Xr = " << X_rand << ", Yr = " << Y_rand << ", Thetar = " << theta_rand << " rad ~ " << theta_rand * (180/CV_PI) << " deg\n");
+ //ROS_INFO_STREAM(" Y_rand = " << Y_rand << " m. \n");
+ //ROS_INFO_STREAM(" theta_rand = " << theta_rand << " rad. =~ " << theta_rand * (180/CV_PI) << " deg. \n");
ROS_INFO_STREAM(" -------------------------------------------------------------- \n");
double vel_x,vel_y,omega_z;
@@ -779,11 +746,11 @@ geometry_msgs::Twist msg_new;
if (X_rand - camPose[2] > x_thresh_undock)
{
ROS_INFO_STREAM(" Adjusting X, moving backward ... \n");
- vel_x = -.02;
+ vel_x = -.04;
} else if (X_rand - camPose[2] < -x_thresh_undock)
{
ROS_INFO_STREAM(" Adjusting X, moving forward ... \n");
- vel_x = .02;
+ vel_x = .04;
}else if (abs(X_rand - camPose[2]) <= x_thresh_undock)
{
ROS_INFO(" X-axis is fixed, adjusting Y & theta - axes ... \n");
@@ -888,4 +855,3 @@ geometry_msgs::Twist msg_new;
commandPub.publish(msg_new);
}
-
diff --git a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
index 62b41767f5e1c8215b801d54d4e487e73aad6ee7..d62ee1be52e512ab9d4b0f205d7d88143fa0bb54 100644
--- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
+++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
@@ -149,9 +149,7 @@ int main()
counter = 1;
print_Q();
generate_rand();
- iterations++;
-
- //Goal = true;
+ iterations++;
} else if (i == P_indx_i && j == P_indx_j) // - Reward => Punishment
{
cout << "\n Failed to achieve a goal! \n";
diff --git a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
index 6b0a52af6a9cab733cde622f93b29c2f0579b857..62b41767f5e1c8215b801d54d4e487e73aad6ee7 100644
--- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
+++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
@@ -34,8 +34,8 @@ double reward;
int R_indx_i = row - row;
int R_indx_j = .5 * col;
-int P_indx_i = row - 7;
-int P_indx_j = col - 10;
+int P_indx_i = row - 2;
+int P_indx_j = col - 1;
int counter = 1;
int Time_Reward;