diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 42d85258a9e57dc334a2749e0fb21d0babb4e1c4..cc66ff08d4a993d988a747ae93ea07d3a06d07c5 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -122,11 +122,9 @@ public:
         static double Pos_Px,Pos_Ix,Pos_Dx;
 	static double Pos_Py,Pos_Iy,Pos_Dy;
 
-	static double S_yPos_P,S_yPos_I,S_yPos_D;
-	static double L_yPos_P,L_yPos_I,L_yPos_D;
-
         static double S_Ang_P,S_Ang_I,S_Ang_D;
         static double L_Ang_P,L_Ang_I,L_Ang_D;
+	static double safety_margin_X;
         
         // ---- CONTROLL PARAMETERS ------ //
         static double prev_errorX;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index 545a6ce6939f9e060dbbe001376fb58171f0358d..42d85258a9e57dc334a2749e0fb21d0babb4e1c4 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -122,8 +122,8 @@ public:
         static double Pos_Px,Pos_Ix,Pos_Dx;
 	static double Pos_Py,Pos_Iy,Pos_Dy;
 
-	//static double S_yPos_P,S_yPos_I,S_yPos_D;
-	//static double L_yPos_P,L_yPos_I,L_yPos_D;
+	static double S_yPos_P,S_yPos_I,S_yPos_D;
+	static double L_yPos_P,L_yPos_I,L_yPos_D;
 
         static double S_Ang_P,S_Ang_I,S_Ang_D;
         static double L_Ang_P,L_Ang_I,L_Ang_D;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 20e7ad16cf91ab2d43c8d1ab6ababeb1f297a86b..42ce8d4097e43359ee289a14ea731ef3c3b53add 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -41,30 +41,21 @@
 using namespace cv;
 using namespace aruco;
 using namespace std;
-
-double ImageConverter::Pos_Px = .00005;
-double ImageConverter::Pos_Ix = 0.0015;
+double ImageConverter::Pos_Px = .05;
+double ImageConverter::Pos_Ix = 0.002;
 double ImageConverter::Pos_Dx = 0;
 
-double ImageConverter::Pos_Py = 2.3 * ImageConverter::Pos_Px;
-double ImageConverter::Pos_Iy = 2.3 * ImageConverter::Pos_Ix;
-double ImageConverter::Pos_Dy = 2.3 * ImageConverter::Pos_Dx;
-
-double ImageConverter::S_yPos_P = 2.3 * ImageConverter::Pos_Px;
-double ImageConverter::S_yPos_I = 2.3 * ImageConverter::Pos_Ix;
-double ImageConverter::S_yPos_D = 2.3 * ImageConverter::Pos_Dx;
+double ImageConverter::Pos_Py = 5 * ImageConverter::Pos_Px;
+double ImageConverter::Pos_Iy = 5 * ImageConverter::Pos_Ix;
+double ImageConverter::Pos_Dy = 5 * ImageConverter::Pos_Dx;
 
-double ImageConverter::L_yPos_P = 15 * ImageConverter::Pos_Px;
-double ImageConverter::L_yPos_I = 15 * ImageConverter::Pos_Ix;
-double ImageConverter::L_yPos_D = 15 * ImageConverter::Pos_Dx;
+double ImageConverter::S_Ang_P = .2 * ImageConverter::Pos_Px;
+double ImageConverter::S_Ang_I = .2 * ImageConverter::Pos_Ix;
+double ImageConverter::S_Ang_D = .2 * ImageConverter::Pos_Dx;
 
-double ImageConverter::S_Ang_P = .062 * ImageConverter::Pos_Px;
-double ImageConverter::S_Ang_I = .062 * ImageConverter::Pos_Ix;
-double ImageConverter::S_Ang_D = .062 * ImageConverter::Pos_Dx;
-
-double ImageConverter::L_Ang_P = .7 * ImageConverter::Pos_Px;
-double ImageConverter::L_Ang_I = .7 * ImageConverter::Pos_Ix;
-double ImageConverter::L_Ang_D = .7 * ImageConverter::Pos_Dx;
+double ImageConverter::L_Ang_P = .8 * ImageConverter::Pos_Px;
+double ImageConverter::L_Ang_I = .8 * ImageConverter::Pos_Ix;
+double ImageConverter::L_Ang_D = .8 * ImageConverter::Pos_Dx;
 
 float ImageConverter::TheMarkerSize = .1; // Default marker size
 
@@ -116,10 +107,13 @@ double ImageConverter::control_signalYAW;
 
 double ImageConverter::zeroMax = .0000000000000000001;
 double ImageConverter::zeroMin = -.0000000000000000001;
+
+// ------ offsets for X, Y, theta
 double ImageConverter::Pz_eps = .001;
-double ImageConverter::Py_eps = .005;
-double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg.
+double ImageConverter::Py_eps = .002;
 
+double ImageConverter::A_eps = (CV_PI/180) * .6; // 1 deg.
+double ImageConverter::safety_margin_X = .15; // 10 cm
 
  ImageConverter::ImageConverter() : 
                                 it_(nh_)
@@ -136,10 +130,6 @@ double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg.
     keepMoving = true;
     commandPub = nh_.advertise<geometry_msgs::Twist>("/base_controller/command",1);
     Sub = nh_.subscribe("/marker_pose",1,&ImageConverter::camCB,this);
-
-
-
-    
   }
 
   ImageConverter::~ImageConverter()
@@ -443,7 +433,7 @@ void ImageConverter::ContStart()
 
 void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
 {
-       
+// Ref. Values
 RefPose[0] = -.0957;
 RefPose[1] = -0.0193642;
 RefPose[2] = 0.307922;
@@ -478,16 +468,34 @@ camPose[3] = CamFB->pose.orientation.x;
         ROS_INFO_STREAM(" ------------------------------------------------------------- \n");
 
 		if (
-            		(abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y
-            		(abs(RefPose[2] - camPose[2]) <= Pz_eps) && // Z
-            		(abs(RefPose[3] - camPose[3]) <= A_eps) // 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
             	)
         	{
                 
                         ROS_INFO("--------/*******//----- Dock is completed ! -----/*********---- \n ");                        
 			keepMoving = false;
-                 
-        	}else
+                 // to make sure that y & theta are within the threshold...
+        	} else if (abs(RefPose[2] - camPose[2]) <= safety_margin_X)
+		{
+			if(
+				(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 ");     			
+				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 FIXED SUCCESSFULLY, MOVING STRAIGHT AHEAD ... \n");
+				Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.01);
+			}
+		}else
         	{
                 	Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.01);
 		}
@@ -571,7 +579,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         		i_termY = S_yPos_I * int_errorY;
         		d_termY = S_yPos_D * diffY;
 		}*/
-
+		
+		ROS_INFO_STREAM("prop_gainY = " << p_termY << ", integ_gainY = " << i_termY << " . \n");	      
         	// control signal
         	control_signalY = p_termY + i_termY + d_termY;
         	
@@ -606,6 +615,8 @@ 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
 		{
@@ -614,6 +625,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         		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");			
@@ -621,6 +633,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         		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");
 		}
         	// control signal
         	control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 30278f8a3b40e3ed1e7ae6b6eb4b17aa0da15d9f..20e7ad16cf91ab2d43c8d1ab6ababeb1f297a86b 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -42,29 +42,25 @@ using namespace cv;
 using namespace aruco;
 using namespace std;
 
-
 double ImageConverter::Pos_Px = .00005;
-double ImageConverter::Pos_Ix = 0.002;
+double ImageConverter::Pos_Ix = 0.0015;
 double ImageConverter::Pos_Dx = 0;
 
+double ImageConverter::Pos_Py = 2.3 * ImageConverter::Pos_Px;
+double ImageConverter::Pos_Iy = 2.3 * ImageConverter::Pos_Ix;
+double ImageConverter::Pos_Dy = 2.3 * ImageConverter::Pos_Dx;
 
-double ImageConverter::Pos_Py = .39 * ImageConverter::Pos_Px;
-double ImageConverter::Pos_Iy = .39 * ImageConverter::Pos_Ix;
-double ImageConverter::Pos_Dy = .39 * ImageConverter::Pos_Dx;
-
-/*
-double ImageConverter::S_yPos_P = .16 * ImageConverter::Pos_Px;
-double ImageConverter::S_yPos_I = .16 * ImageConverter::Pos_Ix;
-double ImageConverter::S_yPos_D = .16 * ImageConverter::Pos_Dx;
+double ImageConverter::S_yPos_P = 2.3 * ImageConverter::Pos_Px;
+double ImageConverter::S_yPos_I = 2.3 * ImageConverter::Pos_Ix;
+double ImageConverter::S_yPos_D = 2.3 * ImageConverter::Pos_Dx;
 
-double ImageConverter::L_yPos_P = .3 * ImageConverter::Pos_Px;
-double ImageConverter::L_yPos_I = .3 * ImageConverter::Pos_Ix;
-double ImageConverter::L_yPos_D = .3 * ImageConverter::Pos_Dx;
-*/
+double ImageConverter::L_yPos_P = 15 * ImageConverter::Pos_Px;
+double ImageConverter::L_yPos_I = 15 * ImageConverter::Pos_Ix;
+double ImageConverter::L_yPos_D = 15 * ImageConverter::Pos_Dx;
 
-double ImageConverter::S_Ang_P = .065 * ImageConverter::Pos_Px;
-double ImageConverter::S_Ang_I = .065 * ImageConverter::Pos_Ix;
-double ImageConverter::S_Ang_D = .065 * ImageConverter::Pos_Dx;
+double ImageConverter::S_Ang_P = .062 * ImageConverter::Pos_Px;
+double ImageConverter::S_Ang_I = .062 * ImageConverter::Pos_Ix;
+double ImageConverter::S_Ang_D = .062 * ImageConverter::Pos_Dx;
 
 double ImageConverter::L_Ang_P = .7 * ImageConverter::Pos_Px;
 double ImageConverter::L_Ang_I = .7 * ImageConverter::Pos_Ix;
@@ -72,8 +68,8 @@ double ImageConverter::L_Ang_D = .7 * ImageConverter::Pos_Dx;
 
 float ImageConverter::TheMarkerSize = .1; // Default marker size
 
-int ImageConverter::Thresh1_min = 20;
-int ImageConverter::Thresh2_min = 20;
+int ImageConverter::Thresh1_min = 10;
+int ImageConverter::Thresh2_min = 10;
 
 int ImageConverter::Thresh1_max = 300;
 int ImageConverter::Thresh2_max = 300;
@@ -120,8 +116,8 @@ double ImageConverter::control_signalYAW;
 
 double ImageConverter::zeroMax = .0000000000000000001;
 double ImageConverter::zeroMin = -.0000000000000000001;
-double ImageConverter::Pz_eps = .005;
-double ImageConverter::Py_eps = .007;
+double ImageConverter::Pz_eps = .001;
+double ImageConverter::Py_eps = .005;
 double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg.
 
 
@@ -140,6 +136,9 @@ double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg.
     keepMoving = true;
     commandPub = nh_.advertise<geometry_msgs::Twist>("/base_controller/command",1);
     Sub = nh_.subscribe("/marker_pose",1,&ImageConverter::camCB,this);
+
+
+
     
   }
 
@@ -446,7 +445,7 @@ void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
 {
        
 RefPose[0] = -.0957;
-RefPose[1] = -0.0203153;
+RefPose[1] = -0.0193642;
 RefPose[2] = 0.307922;
 RefPose[3] = 0.705028;
 
@@ -516,7 +515,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         	diffX = ((curr_errorX - prev_errorX) / dt);
         
         	// scalling
-        	p_termX = Pos_Px  * curr_errorX;
+        	p_termX = Pos_Px * curr_errorX;
         	i_termX = Pos_Ix * int_errorX;
         	d_termX = Pos_Dx * diffX;
         
@@ -532,8 +531,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
 		control_signalX = 0;	
 	}
         // -----------------Y--------------------- // 
-	
-	if(abs(RefY - MarPoseY) > Py_eps)
+	 
+	if((RefY - MarPoseY) <= -Py_eps || (RefY - MarPoseY) >= Py_eps)
 	{	
 		// e(t) = setpoint - actual value;
         	curr_errorY = RefY - MarPoseY;
@@ -555,31 +554,35 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         	i_termY = Pos_Iy * int_errorY;
         	d_termY = Pos_Dy * diffY;
 
-		/*int  y_offset = 5;
-		if (curr_errorY < y_offset && curr_errorY > -y_offset) // -5 < err < +5
+		/*double  x_offset = .25;
+		if (abs(curr_errorY) >= abs(curr_errorX)) // -5 < err < +5
 		{
-			ROS_INFO_STREAM(" INSIDE boundry of " << y_offset << " m. \n");	        		
+			ROS_INFO_STREAM("robot movement in Y-direction is dominant! \n");	        		
+			control_signalX = 0;				
 			// scalling
-        		p_termYAW = S_yPos_P * curr_errorY;
-        		i_termYAW = S_yPos_I * int_errorY;
-        		d_termYAW = S_yPos_D * diffY;
-        	} else if (curr_errorY > y_offset || curr_errorYAW < y_offset) //  err > +5 or err < -5
+        		p_termY = L_yPos_P * curr_errorY;
+        		i_termY = L_yPos_I * int_errorY;
+        		d_termY = L_yPos_D * diffY;
+        	} else
 		{
-			ROS_INFO_STREAM(" OUTSIDE boundry of " << y_offset << " m. \n");			
+			ROS_INFO_STREAM("robot movement in X-direction is dominant!  \n");			
 			// scalling
-        		p_termYAW = L_yPos_P * curr_errorY;
-        		i_termYAW = L_yPos_I * int_errorY;
-        		d_termYAW = L_yPos_D * diffY;
+        		p_termY = S_yPos_P * curr_errorY;
+        		i_termY = S_yPos_I * int_errorY;
+        		d_termY = S_yPos_D * diffY;
 		}*/
 
         	// control signal
         	control_signalY = p_termY + i_termY + d_termY;
-        
-        
+        	
+		// robot & marker coordinates conversion
+        	control_signalY = - control_signalY;
+		
         	// save the current error as the previous one
         	// for the next iteration.
-        	prev_errorY = curr_errorY;      
-        } else
+        	prev_errorY = curr_errorY;
+	
+	}else if ((RefY - MarPoseY) < Py_eps && (RefY - MarPoseY) > -Py_eps)
 	{
 		control_signalY = 0;	
 	}
@@ -644,7 +647,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
 	//ROS_INFO_STREAM("RAW Control signalYAW = "<< control_signalYAW <<". \n");
 	ROS_INFO_STREAM(" ------------------------------------------------------------- \n");	
 	
-	dock(-control_signalX, -control_signalY, control_signalYAW);
+	dock(-control_signalX, control_signalY, control_signalYAW);
 	
 }
 
@@ -692,4 +695,4 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega
 	
 	ROS_INFO_STREAM(" Current ESTIMATED speed of robot: \n" << msg << ".\n");
 }
-// ---- Controller part ----------- END --------
+// ---- Controller part -------- END ------