diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index c30473c2c114ddb863b300c90d5033a936f79d39..933df333ab0f9ae73fd04550d31cef12f83af6ad 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);
 			}
@@ -494,13 +494,10 @@ camPose[3] = CamFB->pose.orientation.x;
 	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(" 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(" Ymar = " << camPose[1] << " m. \n");
-        ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \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 +509,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 +542,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 +626,6 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
 	}
         // -------------------YAW--------------------------//
         
-
 	if(abs(RefYAW - abs(MarPoseYAW)) > A_eps)
 	{	
 		// e(t) = setpoint - actual value;
@@ -669,7 +664,6 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
 			//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 +696,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 +752,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 +772,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 +881,3 @@ 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 7866151018fa7c2cd8d9107b875eeaa8f4dce567..c30473c2c114ddb863b300c90d5033a936f79d39 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -103,16 +103,20 @@ double ImageConverter::Pos_Ix = 0.0028;
 double ImageConverter::Pos_Dx = 0;
 
 double ImageConverter::Pos_Py = 4 * Pos_Px;
-double ImageConverter::Pos_Iy = 4 * Pos_Ix;
+double ImageConverter::Pos_Iy = .5 * Pos_Ix;
 double ImageConverter::Pos_Dy = 4 * Pos_Dx;
 
-double ImageConverter::S_Ang_P = .2 * Pos_Px;
+/*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::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::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration;
 
@@ -121,8 +125,8 @@ double ImageConverter::zeroMin = -.0000000000000000001;
 
 // ------ offsets X, Y, theta for Docking ---------
 double ImageConverter::Pz_eps = .001;
-double ImageConverter::Py_eps = .002;
-double ImageConverter::A_eps = (CV_PI/180) * .6; // 1 deg.
+double ImageConverter::Py_eps = .0015;
+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
 
@@ -134,6 +138,7 @@ double ImageConverter::theta_thresh_undock = (CV_PI/180) * 3;
 // random pose initialized
 double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new;
 
+double ImageConverter::docking_counter = 1;
  ImageConverter::ImageConverter() : 
                                 it_(nh_)
   {
@@ -146,13 +151,12 @@ double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new;
 // Ref. Values
 RefPose[0] = -.0957;
 
-RefPose[1] = -0.0194805;
-RefPose[2] = 0.306654;
+RefPose[1] = -0.0196805;
+RefPose[2] = 0.306154;
 RefPose[3] = 0.702366;
 
 x_rand_SM = RefPose[2] + .45; // 45 cm spreading domain in the x-axis while moving towards the random pose
 
-
      //marSub = nh_.subscribe("/Marker_pose",1,&ImageConverter::marCB,this);
      
     // Publish pose message and buffer up to 100 messages
@@ -246,7 +250,6 @@ bool ImageConverter::readArguments ( int argc,char **argv )
 
 void ImageConverter::ProgStart(int argc,char** argv)
 {
-
 	// Show images, press "SPACE" to diable image
         // rendering to save CPU time
         
@@ -317,7 +320,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
 			{
 			        found = false;
 				keepMoving = false;
-				ROS_INFO("SORRY, BUT MARKER IS LOST, Starting again ... \n");					
+				ROS_INFO_STREAM("damn, marker is lost, but " << docking_counter<< " successful docking trials ... \n");					
 				//RandomPose(x_new,y_new,theta_new);			
 				//move2docking(-control_signalX, -control_signalY, control_signalYAW);
 			}
@@ -487,9 +490,7 @@ camPose[3] = CamFB->pose.orientation.x;
 	/*ROS_INFO_STREAM(" --------------------- TIMER ------------------ \n");
 	ROS_INFO_STREAM("Docking duration : "<< hr << " : " << min << " : " << sec << " . " << msec << "\n");*/
 	
-
-	
-	ROS_INFO_STREAM(" ------------------------- GAINS ---------------------- \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");
@@ -515,9 +516,10 @@ camPose[3] = CamFB->pose.orientation.x;
                         //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("Robot is docked in " << docking_duration <<" sec, Moving 2 new Random Pose\n");			
+			ROS_INFO_STREAM("docking No. " << docking_counter << " is finished in "<< docking_duration <<" sec, Moving 2 new Random Pose\n");		
 			//keepMoving = false;
 			GenerateRandomVal();
+			docking_counter ++;
 			Go2RandomPose = true;
 
 		// to make sure that y & theta are within the threshold...
@@ -527,16 +529,15 @@ camPose[3] = CamFB->pose.orientation.x;
 				(abs(RefPose[1] - camPose[1]) > Py_eps) || 
 				(abs(RefPose[3] - abs(camPose[3])) > A_eps)
 			)
-			{			
-				ROS_INFO_STREAM(" X < " << safety_margin_X << " m. , Fixing Y and theta. \n ");     			
+			{	
+				ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m. , Fixing Y or theta. \n ");     			
 				Controller(RefPose[2], RefPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.01);
-
 			} else if(
 				(abs(RefPose[1] - camPose[1]) <= Py_eps) && 
 				(abs(RefPose[3] - abs(camPose[3])) <= A_eps)				
 				)
 			{
-				ROS_INFO("y & theta have been fixed successfully, MOVING STRAIGHT AHEAD ... \n");
+				ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
 				Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.01);
 			}
 		}else
@@ -590,7 +591,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
 	}
         // -----------------Y--------------------- // 
 	 
-	if((RefY - MarPoseY) <= -Py_eps || (RefY - MarPoseY) >= Py_eps)
+	if((RefY - MarPoseY) < -Py_eps || (RefY - MarPoseY) > Py_eps)
 	{	
 		// e(t) = setpoint - actual value;
         	curr_errorY = RefY - MarPoseY;
@@ -611,7 +612,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         	p_termY = Pos_Py * curr_errorY;
         	i_termY = Pos_Iy * int_errorY;
         	d_termY = Pos_Dy * diffY;
-		//ROS_INFO_STREAM("prop_gainY = " << p_termY << ", integ_gainY = " << i_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;
         	
@@ -622,7 +624,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         	// for the next iteration.
         	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;	
 	}
@@ -647,7 +649,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         	diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
         	
 		
-		// YAW offset...
+		/*// YAW offset...
 		int  yaw_offset = 15;
 		if (curr_errorYAW < ((CV_PI/180) * yaw_offset) && curr_errorYAW > ((CV_PI/180) * -yaw_offset)) // -5 < err < +5
 		{
@@ -665,8 +667,16 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         		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");
-		}
-        	// control signal
+		}*/
+
+		
+		ROS_INFO_STREAM(" adjusting orientation! \n");	        		
+		// scalling
+        	p_termYAW = Ang_P * curr_errorYAW;
+       		i_termYAW = Ang_I * int_errorYAW;
+       		d_termYAW = Ang_D * diffYAW;
+        	
+		// control signal
         	control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
         	
 		if(MarPoseYAW < 0)
@@ -744,13 +754,13 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega
 void ImageConverter::GenerateRandomVal()
 {
 	// ---------------- PID gains ------------------
-	Pos_Px = ((double) rand() / (RAND_MAX)) * (.16 - .09) + .09; 	//   .09 < Kp < .16
+	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 < .02
+	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)) * (.59 - (-.24)) + (-.24);
+	y_new = ((double) rand() / (RAND_MAX)) * (.5 - (-.24)) + (-.24);
 	theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]);
 }