diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 0adcc11bb7fcc297988905c626c3e70ddaaf0ce8..48e600a4767544951c20cf0bab579d487b843cb5 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -36,7 +36,7 @@ using namespace cv;
 using namespace aruco;
 using namespace std;
 
-class VisCont
+class PID4Docking
 {
 private:
 
@@ -98,9 +98,9 @@ private:
         
 public:
 
-  VisCont();
+  PID4Docking();
     
-  ~VisCont();
+  ~PID4Docking();
   
   Mat getCurrentImage();
 
@@ -159,42 +159,10 @@ public:
         static double control_signalYAW;
         // ---- CONTROLL PARAMETERS ------ //
         
-	        
-	// ---- Q_LEARNING PARAMETERS ------ //
-
 	static double x_new,y_new,theta_new; 	// inp (y,theta)
 	static const double y_up,y_dwn,theta_up,theta_dwn;
 	static double Kp_y,Ki_y,Kd_y; 		// actions (Kp,Ki,Kd)
 	
-	static int R_indx_i, R_indx_j, P_indx_i, P_indx_j;
-
-
-	static const int row, col;
-	static double gamma;
-	static double alpha;
-	static double R_step;
-	
-	static double R[7][7];
-	static double Q[7][7];
-
-	static double Q_curr_state,Q_next_state;
-	static double reward;
-	static int counter;
-	static int Time_Reward;
-	static double sample;
-
-	void Q_Learning(double y, double theta);
-	void print_Q();
-
-	void print_R();
-
-	// ---- Q_LEARNING PARAMETERS ------ //
-
-
-	// ---- TIMER PARAMETERS ------ //
-	static int msec,sec,min,hr;
-	// ---- TIMER PARAMETERS ------ //
-
 	static double docking_counter;
 	static double dock_started,dock_finished,docking_duration;
 	
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index 20512a5a867f304c32ee97ac8b01af91f79bedbc..79669717f20648fb3ee78c6af549a6508ca49b5b 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -36,7 +36,7 @@ using namespace cv;
 using namespace aruco;
 using namespace std;
 
-class VisCont
+class PID4Docking
 {
 private:
 
@@ -98,9 +98,9 @@ private:
         
 public:
 
-  VisCont();
+  PID4Docking();
     
-  ~VisCont();
+  ~PID4Docking();
   
   Mat getCurrentImage();
 
@@ -159,42 +159,10 @@ public:
         static double control_signalYAW;
         // ---- CONTROLL PARAMETERS ------ //
         
-	        
-	// ---- Q_LEARNING PARAMETERS ------ //
-
 	static double x_new,y_new,theta_new; 	// inp (y,theta)
 	static const double y_up,y_dwn,theta_up,theta_dwn;
 	static double Kp_y,Ki_y,Kd_y; 		// actions (Kp,Ki,Kd)
 	
-	static int R_indx_i, R_indx_j, P_indx_i, P_indx_j;
-
-
-	static const int row, col;
-	static double gamma;
-	static double alpha;
-	static double R_step;
-	
-	static double R[7][7];
-	static double Q[7][7];
-
-	static double Q_curr_state,Q_next_state;
-	static double reward;
-	static int counter;
-	static int Time_Reward;
-	static double sample;
-
-	void Q_Learning(double y, double theta);
-	void print_Q();
-
-	void print_R();
-
-	// ---- Q_LEARNING PARAMETERS ------ //
-
-
-	// ---- TIMER PARAMETERS ------ //
-	static int msec,sec,min,hr;
-	// ---- TIMER PARAMETERS ------ //
-
 	static double docking_counter;
 	static double dock_started,dock_finished,docking_duration;
 	
@@ -204,10 +172,9 @@ 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;
+	static double speed_reducer_X,speed_reducer_Y,speed_reducer_theta;
 
         double marpose[6];
         double camPose[6];
         static const double RefPose[4];
         
-};
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 346501556df8217e0eaea508e6d422c219b6f99d..94c0e546693547263372b475ddcffd33e0863a53 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -42,140 +42,113 @@
 
 #include<VisionControl.h>
 
+
+
 using namespace cv;
 using namespace aruco;
 using namespace std;
 
-float VisCont::TheMarkerSize = .1; // Default marker size
+float PID4Docking::TheMarkerSize = .1; // Default marker size
 
-int VisCont::Thresh1_min = 10;
-int VisCont::Thresh2_min = 10;
+int PID4Docking::Thresh1_min = 10;
+int PID4Docking::Thresh2_min = 10;
 
-int VisCont::Thresh1_max = 300;
-int VisCont::Thresh2_max = 300;
+int PID4Docking::Thresh1_max = 300;
+int PID4Docking::Thresh2_max = 300;
 
-const string VisCont::trackbarWindowName = "Trackbars";
+const string PID4Docking::trackbarWindowName = "Trackbars";
 
-int VisCont::ThePyrDownLevel = 0;
+int PID4Docking::ThePyrDownLevel = 0;
 
-bool VisCont::update_images = true;
+bool PID4Docking::update_images = true;
 
-bool VisCont::found = false;
-bool VisCont::Go2RandomPose = false;
+bool PID4Docking::found = false;
+bool PID4Docking::Go2RandomPose = false;
 
-int VisCont::ThresParam1 = 0;
-int VisCont::ThresParam2 = 0;
+int PID4Docking::ThresParam1 = 0;
+int PID4Docking::ThresParam2 = 0;
 
 // ---- CONTROLL PARAMETERS ------ //
-double VisCont::prev_errorX, VisCont::curr_errorX, VisCont::int_errorX, VisCont::diffX;
+double PID4Docking::prev_errorX, PID4Docking::curr_errorX, PID4Docking::int_errorX, PID4Docking::diffX;
 
-double VisCont::p_termX = 0;
-double VisCont::i_termX = 0;
-double VisCont::d_termX = 0;
+double PID4Docking::p_termX = 0;
+double PID4Docking::i_termX = 0;
+double PID4Docking::d_termX = 0;
 
-double VisCont::prev_errorY, VisCont::curr_errorY, VisCont::int_errorY, VisCont::diffY;
+double PID4Docking::prev_errorY, PID4Docking::curr_errorY, PID4Docking::int_errorY, PID4Docking::diffY;
 
-double VisCont::p_termY = 0;
-double VisCont::i_termY = 0;
-double VisCont::d_termY = 0;
+double PID4Docking::p_termY = 0;
+double PID4Docking::i_termY = 0;
+double PID4Docking::d_termY = 0;
 
-double VisCont::prev_errorYAW,VisCont::curr_errorYAW,VisCont::int_errorYAW,VisCont::diffYAW;
+double PID4Docking::prev_errorYAW,PID4Docking::curr_errorYAW,PID4Docking::int_errorYAW,PID4Docking::diffYAW;
 
-double VisCont::p_termYAW = 0;
-double VisCont::i_termYAW = 0;
-double VisCont::d_termYAW = 0;
+double PID4Docking::p_termYAW = 0;
+double PID4Docking::i_termYAW = 0;
+double PID4Docking::d_termYAW = 0;
 
-double VisCont::control_signalX, VisCont::control_signalY, VisCont::control_signalYAW;
+double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::control_signalYAW;
 // ---- CONTROLL PARAMETERS ------ //
 
 
 // ---- Ref. Values for Android Camera ---- //
-//const double VisCont::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ ,  0.308857 /* X_ref*/ , 0.17 /* theta_ref*/}; 
+//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 VisCont::RefPose[4] = {-.0957, -0.0271482 /* Y_ref*/ ,  0.266214 /* X_ref*/ , -0.535344 /* theta_ref*/}; 
-
+const double PID4Docking::RefPose[4] = {-.0957, -0.0295876 /* Y_ref*/ ,  0.218695 /* X_ref*/ , -0.637745 /* theta_ref*/}; 
 
 // ----------------  PID gains---------------- //
-double VisCont::Kp_y = .7; //.7
-double VisCont::Ki_y = .005;//.005
-double VisCont::Kd_y = .2;
+double PID4Docking::Kp_y = .7; //.7
+double PID4Docking::Ki_y = .001;//.005
+double PID4Docking::Kd_y = .2;
 
-double VisCont::Kp_theta = .1 * Kp_y; // .08 * Kp_y
-double VisCont::Ki_theta = .07 * Ki_y;
-double VisCont::Kd_theta = .00005 * Kd_y;
+double PID4Docking::Kp_theta = .2 * Kp_y;
+double PID4Docking::Ki_theta = .6 * Ki_y; // .07 * Ki_y
+double PID4Docking::Kd_theta = .00005 * Kd_y;
 // ----------------  PID gains---------------- //
 
 
-double VisCont::TT_S,VisCont::TT_E;
+double PID4Docking::TT_S,PID4Docking::TT_E;
 // random pose initialized
-const double VisCont::y_up = .3; 
-const double VisCont::y_dwn = -.1; 
-
-const double VisCont::theta_dwn = -.7 /*-RefPose[3]*/; 
-const double VisCont::theta_up = .7 /*RefPose[3]*/;
-
-double VisCont::x_new,VisCont::y_new,VisCont::theta_new;
-
-// ----------------  Q_LEARNING PARAMETERS ---------------- //
-
-double VisCont::gamma = .8;
-double VisCont::alpha = .1;
-double VisCont::R_step = 200;
+const double PID4Docking::y_up = .3; 
+const double PID4Docking::y_dwn = -.1; 
 
-const int VisCont::row = 5/*(y_up - y_dwn) * 10000*/;
-const int VisCont::col = 5/*(theta_up - theta_dwn) * 1000*/;
+const double PID4Docking::theta_dwn = -.7 /*-RefPose[3]*/; 
+const double PID4Docking::theta_up = .7 /*RefPose[3]*/;
 
-double VisCont::Q[7][7] = {0};
-double VisCont::R[7][7] = {0};
+double PID4Docking::x_new,PID4Docking::y_new,PID4Docking::theta_new;
 
-double VisCont::Q_next_state;
 
-int VisCont::counter = 1;
-int VisCont::Time_Reward;
-double VisCont::sample;
+double PID4Docking::dock_started,PID4Docking::dock_finished,PID4Docking::docking_duration;
+double PID4Docking::zeroMax = .0000000000000000001;
+double PID4Docking::zeroMin = -.0000000000000000001;
 
-int VisCont::R_indx_i = 0;
-int VisCont::R_indx_j = 2;
-
-int VisCont::P_indx_i = 1;
-int VisCont::P_indx_j = 1;
-// ----------------  Q_LEARNING PARAMETERS ---------------- //
-
-double VisCont::dock_started,VisCont::dock_finished,VisCont::docking_duration;
-double VisCont::zeroMax = .0000000000000000001;
-double VisCont::zeroMin = -.0000000000000000001;
-
-double VisCont::speed_reducer_X = 1;
-double VisCont::speed_reducer_Y = 1;
-double VisCont::speed_reducer_theta = 1;
+double PID4Docking::speed_reducer_X = 1;
+double PID4Docking::speed_reducer_Y = 1;
+double PID4Docking::speed_reducer_theta = 1;
 
 // ------ offsets X, Y, theta for Docking ---------
-double VisCont::Pz_eps = .001;
-double VisCont::Py_eps = .005;
-double VisCont::A_eps = (CV_PI/180) * 6; // 1 deg.
+double PID4Docking::Pz_eps = .001;
+double PID4Docking::Py_eps = .002;
+double PID4Docking::A_eps = (CV_PI/180) * 1; // 1 deg.
 
-double VisCont::safety_margin_X = .2; // safety margin X axis in docking process : 12 cm
+double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
 
 // ------ offsets X, Y, theta for Undocking ---------
-double VisCont::x_thresh_undock = .02;
-double VisCont::y_thresh_undock = .015;
-double VisCont::theta_thresh_undock = (CV_PI/180) * 3;
+double PID4Docking::x_thresh_undock = .02;
+double PID4Docking::y_thresh_undock = .015;
+double PID4Docking::theta_thresh_undock = (CV_PI/180) * 3;
 
 
-double VisCont::docking_counter = 1;
+double PID4Docking::docking_counter = 1;
 
-VisCont::VisCont()
+PID4Docking::PID4Docking()
 {	
 	keepMoving = true;    
     
 	/* initialize random seed: */
-  	//srand (time(NULL));
-
-
-	R[R_indx_i][R_indx_j] = 50; // reward
-        R[P_indx_i][P_indx_j] = -60; // punishment
+  	srand (time(NULL));
 
 	x_rand_SM = RefPose[2] + .55; // 55 cm spreading domain in the x-axis while moving towards the random pose
 
@@ -183,25 +156,25 @@ VisCont::VisCont()
     	MarPose_pub = node_vis.advertise<geometry_msgs::PoseStamped>("/marker_pose", 100);
     	commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",1);
     
-    	MarPose_Sub = node_vis.subscribe("/marker_pose",1,&VisCont::camCB,this);
+    	MarPose_Sub = node_vis.subscribe("/marker_pose",1,&PID4Docking::camCB,this);
 }
 
-  VisCont::~VisCont()
+  PID4Docking::~PID4Docking()
   {
 	
   }
-  Mat VisCont::getCurrentImage() 
+  Mat PID4Docking::getCurrentImage() 
   {
         return img;
   }
 
-void VisCont::cvTackBarEvents(int value,void* ptr)
+void PID4Docking::cvTackBarEvents(int value,void* ptr)
 {
-    VisCont* ic = (VisCont*)(ptr);
+    PID4Docking* ic = (PID4Docking*)(ptr);
     ic-> myhandler(value);
 }
 
-void VisCont::myhandler(int value)
+void PID4Docking::myhandler(int value)
 {
         if (Thresh1_min<3) Thresh1_min=3;
     
@@ -227,14 +200,14 @@ void VisCont::myhandler(int value)
     //imshow("INPUT IMAGE",TheInputImageCopy);
     //imshow("THRESHOLD IMAGE",MDetector.getThresholdedImage());
 }
-void VisCont::createTrackbars()
+void PID4Docking::createTrackbars()
 {    
 	namedWindow(trackbarWindowName, 0);
 	createTrackbar("ThresParam 1", trackbarWindowName, &Thresh1_min, Thresh1_max, cvTackBarEvents, this);
 	createTrackbar("ThresParam 2", trackbarWindowName, &Thresh2_min, Thresh2_max, cvTackBarEvents, this);
 	
 }
-void VisCont::imageCb(const sensor_msgs::ImageConstPtr& msg)
+void PID4Docking::imageCb(const sensor_msgs::ImageConstPtr& msg)
 {
     cv_bridge::CvImagePtr cv_ptr;
     try
@@ -249,7 +222,7 @@ void VisCont::imageCb(const sensor_msgs::ImageConstPtr& msg)
     img = cv_ptr->image;
     
 }
-bool VisCont::readArguments ( int argc,char **argv )
+bool PID4Docking::readArguments ( int argc,char **argv )
 {
     if (argc<2) {
         cerr<< "Invalid number of arguments!\n" <<endl;
@@ -266,7 +239,7 @@ bool VisCont::readArguments ( int argc,char **argv )
     return true;
 }
 
-void VisCont::ProgStart(int argc,char** argv)
+void PID4Docking::ProgStart(int argc,char** argv)
 {
 	// Show images, press "SPACE" to diable image
         // rendering to save CPU time
@@ -357,10 +330,10 @@ if (TheVideoCapturer.retrieve(TheInputImage))
 			x_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
 			z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2];
 		
-			cv::Mat R(3,3,cv::DataType<float>::type);
+			Mat R(3,3,cv::DataType<float>::type);
 
 			// You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords
-			cv::Rodrigues(TheMarkers[0].Rvec,R);
+			Rodrigues(TheMarkers[0].Rvec,R);
 
                                 // ----------- Euler angle -----------------//
                                 float roll1 = -asin(R.at<float>(2,0));
@@ -459,15 +432,13 @@ if (key == ' ')
 }
 
 		ros::spinOnce();
-		//rate.sleep();
-TT_E = ros::Time::now().toSec();
-ROS_INFO_STREAM("start = "<< TT_S << " , end = " << TT_E << " , duration = " << (TT_E - TT_S) <<" \n");
-
-	}
 		
+		TT_E = ros::Time::now().toSec();
+		ROS_INFO_STREAM(" visualization while loop duration = " << (TT_E - TT_S) <<" \n");
 
+	}
 }
-void VisCont::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber
+void PID4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber
 {
 camPose[0] = CamFB->pose.position.x; // not important!!!
 camPose[1] = CamFB->pose.position.y; // y pose
@@ -539,9 +510,6 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
 		}else
         	{
                 	Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
-			//Q_Learning(camPose[1],camPose[3]); // performing Q_Learning...
-			//print_R();
-
 		}
 	} else
 	{
@@ -550,7 +518,7 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
 	}
 }
 
-void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
+void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
 {
 	ROS_INFO_STREAM("--------------------- Controller started ----------------------\n "); 
         
@@ -659,11 +627,10 @@ void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPo
 	//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 VisCont::dock(double VelX, double VelY, double omegaZ)
+void PID4Docking::dock(double VelX, double VelY, double omegaZ)
 {
         ROS_INFO(".... REAL .... !");
         geometry_msgs::Twist msg;
@@ -677,7 +644,7 @@ void VisCont::dock(double VelX, double VelY, double omegaZ)
 	ROS_INFO_STREAM(" Current speed of robot: \n" << msg << ".\n");
 }
 
-void VisCont::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
+void PID4Docking::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
 {
 	
         ROS_INFO_STREAM(" Zmar = " << camPose[2] << " m. \n");
@@ -709,7 +676,7 @@ void VisCont::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
 }
 // ---- Controller part -------- END ------
 
-void VisCont::GenerateRandomVal()
+void PID4Docking::GenerateRandomVal()
 {
 
 	// ---------------- PID gains ------------------
@@ -724,7 +691,7 @@ void VisCont::GenerateRandomVal()
 	theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn; // will be used for Q_Learning
 }
 
-void VisCont::RandomPose(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");
@@ -827,153 +794,3 @@ geometry_msgs::Twist msg_new;
 commandPub.publish(msg_new);
 	
 }
-
-void VisCont::Q_Learning(double y, double theta)
-{
-
-/*	while ( iterations <= it_ ) 
-        {       
-                if (user_action == 1 && y_new != 0) // North
-                {
-                        reward = R[y_new][theta_new];
-                        Q_next_state = Q[y_new - 1][theta_new];
-                        
-                        sample = reward + gamma * Q_next_state;
-                        Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
-                        Q_curr_state = Q_next_state;
-                        i--;
-                        counter++;
-                } else if (user_action == 1 && y_new == 0) // North
-                {
-                        cout << "You can't go further up!\n";
-                } else if (user_action == 3 && i < (row - 1)) // South, i < row
-                {
-                        reward = R[y_new][theta_new];
-                        Q_next_state = Q[y_new + 1][theta_new];
-
-
-
-                        
-                        sample = reward + gamma * Q_next_state;
-                        Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
-                        Q_curr_state = Q_next_state;
-                        i++;
-                        counter++;
-                } else if (user_action == 3 && y_new >= (row - 1)) // South
-                {
-                        cout << "You can't go further down!\n";
-                } else if (user_action == 2 && theta_new < (col - 1)) // East
-                {
-                        reward = R[y_new][theta_new];
-                        Q_next_state = Q[i][j + 1];
-                        
-                        sample = reward + gamma * Q_next_state;
-                        Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
-                        Q_curr_state = Q_next_state;
-                        j++;
-                        counter++;
-                } else if (user_action == 2 && j >= (col - 1)) // East, j > col
-                {
-                        cout << "You can't go further right!\n";
-                } else if (user_action == 4 && j != 0 ) // West
-                {
-                        reward = R[y_new][theta_new];
-                        Q_next_state = Q[y_new][theta_new - 1];
-                        
-                        sample = reward + gamma * Q_next_state;
-                        Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
-                        Q_curr_state = Q_next_state;
-                        j--;
-                        counter++;
-                } else if (user_action == 4 && theta_new == 0) // West, j = 1
-                {
-                        cout << "You can't go further left!\n";
-                } else
-                {
-                        cout << "\nGenerating random pose in grid for 1st. time!\n";
-                        generate_rand();
-                }
-                
-                // + Reward
-                if (y_new == R_indx_i && theta_new == R_indx_j)
-                {
-                        Time_Reward = -counter;
-                        cout << " Time Reward = "<< Time_Reward << "\n";
-                        
-                        if(abs(Time_Reward) <= R_step)
-                        {
-                                
-                                cout << "\n Goal is achieved <= " << R_step << " time steps\n";
-                                reward = R[y_new][theta_new];
-                                Q_next_state = 0;
-                        
-                                sample = reward + gamma * Q_next_state;
-                                Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
-                        } else
-                        {
-                                cout << "\n Goal is achieved > " << R_step << " time steps => time_punishment\n";
-                                reward = -1; // ???
-                                Q_next_state = 0;
-                        
-                                sample = reward + gamma * Q_next_state;
-                                Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
-                        }
-                        
-                        counter = 0;  
-                } else if (y_new == P_indx_i && theta_new == P_indx_j) // (-) Reward => Punishment
-                {
-                           cout << "\n Failed to achieve a goal! \n";
-                                
-                                reward = R[y_new][theta_new];
-                                Q_next_state = 0;
-                        
-                                sample = reward + gamma * Q_next_state;
-                                Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);  
-                }
-                
-                cout << "\n Q_value = " << Q_curr_state << " , actions N(1), E(2), S(3), W(4) : ";
-                user_action = ((double) rand() / (RAND_MAX)) * (5 - 1) + 1; // Generating random user_action  
-                printf(" ramdom user action = %i \n",user_action);
-                //cin >> user_action;
-          }*/
-}
-
-
-void VisCont::print_R()
-{
-         cout << " R = \n";
-        for(int i = 0; i <= (row - 1); i++)
-        {
-                for(int j = 0; j <= (col - 1); j++)
-                {
-                        cout << setw(col - 1) << R[i][j];
-			if(j < col - 1)
-			{
-				cout << " , ";
-			}
-		} // j
-                cout << "\n";
-        } // i
-        cout << "\n";
-}
-
-
-void VisCont::print_Q()
-{
-         cout << " Q = \n";
-        for(int i = 0; i <= (row - 1); i++)
-        {
-                for(int j = 0; j <= (col - 1); j++)
-                {
-                        cout << setw(col - 1) << Q[i][j];
-			if(j < col - 1)
-			{
-				cout << " , ";
-			}
-		} // j
-                cout << "\n";
-        } // i
-        cout << "\n";
-}
-
-
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 9d8c341089623c3314e750ddcc45347ab96c4548..346501556df8217e0eaea508e6d422c219b6f99d 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -98,12 +98,12 @@ const double VisCont::RefPose[4] = {-.0957, -0.0271482 /* Y_ref*/ ,  0.266214 /*
 
 
 // ----------------  PID gains---------------- //
-double VisCont::Kp_y = .5; //.6
-double VisCont::Ki_y = .05;
-double VisCont::Kd_y = .3;
+double VisCont::Kp_y = .7; //.7
+double VisCont::Ki_y = .005;//.005
+double VisCont::Kd_y = .2;
 
 double VisCont::Kp_theta = .1 * Kp_y; // .08 * Kp_y
-double VisCont::Ki_theta = .002 * Ki_y;
+double VisCont::Ki_theta = .07 * Ki_y;
 double VisCont::Kd_theta = .00005 * Kd_y;
 // ----------------  PID gains---------------- //
 
@@ -153,10 +153,10 @@ double VisCont::speed_reducer_theta = 1;
 
 // ------ offsets X, Y, theta for Docking ---------
 double VisCont::Pz_eps = .001;
-double VisCont::Py_eps = .004;
+double VisCont::Py_eps = .005;
 double VisCont::A_eps = (CV_PI/180) * 6; // 1 deg.
 
-double VisCont::safety_margin_X = .18; // safety margin X axis in docking process : 12 cm
+double VisCont::safety_margin_X = .2; // safety margin X axis in docking process : 12 cm
 
 // ------ offsets X, Y, theta for Undocking ---------
 double VisCont::x_thresh_undock = .02;
@@ -533,7 +533,7 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
 				)
 			{
 				ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
-				speed_reducer_X = .1;
+				speed_reducer_X = .08;
 				Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.1);
 			}
 		}else
@@ -571,10 +571,10 @@ void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPo
         	// control signal
         	control_signalX = p_termX + i_termX + d_termX;
         	prev_errorX = curr_errorX;*/
-		control_signalX = speed_reducer_X * 0.08;        
+		control_signalX = speed_reducer_X * 0.1;        
         } else
 	{
-		control_signalX = 1e-4;	
+		control_signalX = 5e-5;	
 	}
         // -----------------Y--------------------- // 
 	 
@@ -613,7 +613,8 @@ void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPo
 	{
 		control_signalY = 0;	
 	}
-        // -------------------YAW--------------------------//
+        
+	// -------------------YAW--------------------------//
        if(abs(abs(RefYAW) - abs(MarPoseYAW)) > A_eps)
 	{	
 		// e(t) = setpoint - actual value;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp
index 6c52b0ba8d82cd384dd013e32eaa19b16f643d63..fbdef348c6270c7c94717a50c3faa0863d6a032b 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp
@@ -5,12 +5,8 @@ int main(int argc, char** argv)
 {
       ros::init(argc, argv, "aruco_tf_publisher");
         
-        VisCont imgConv;
-        imgConv.ProgStart(argc,argv);
-	
-	
+        PID4Docking controller;
+        controller.ProgStart(argc,argv);
 
         return 0;
-
-
 }
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~
index 6c52b0ba8d82cd384dd013e32eaa19b16f643d63..98e8f518644e44d67a46013115ebe0e1616ea1c4 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~
@@ -5,12 +5,12 @@ int main(int argc, char** argv)
 {
       ros::init(argc, argv, "aruco_tf_publisher");
         
-        VisCont imgConv;
-        imgConv.ProgStart(argc,argv);
+        PID4Docking controller;
+        controller.ProgStart(argc,argv);
 	
+/*PID4Docking cont;
+cont.ContStart();*/
 	
 
         return 0;
-
-
 }
diff --git a/MobileRobot/Machine_Learning/Practice/Q_Learning b/MobileRobot/Machine_Learning/Practice/Q_Learning
index 0d0d3255a39c1b2f6342705481ca0d3e9dd01f59..63742263fbc07be2b1b8928dd5f6787fced58998 100755
Binary files a/MobileRobot/Machine_Learning/Practice/Q_Learning and b/MobileRobot/Machine_Learning/Practice/Q_Learning differ
diff --git a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
index 1e1d2a7d33b295f262fa8b6d7be2612f7a414524..850990ed6d2e0bed253b6905a7b690b991926a12 100644
--- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
+++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
@@ -39,7 +39,7 @@ double sample;
 
 void print_R();
 void print_Q();
-void generate_rand();
+void iANDj_Generator();
 
 int main()
 {
@@ -115,7 +115,7 @@ int main()
                 } else
                 {
                         cout << "\nGenerating random pose in grid for 1st. time!\n";
-                        generate_rand();
+                        iANDj_Generator();
                 }
                 
                 // + Reward
@@ -145,7 +145,7 @@ int main()
                         
                         counter = 0;
                         print_Q();
-                        generate_rand();
+                        iANDj_Generator();
                         iterations++;    
                 } else if (i == P_indx_i && j == P_indx_j) // - Reward => Punishment
                 {
@@ -158,7 +158,7 @@ int main()
                                 Q[i][j] = ((1 - alpha) * Q[i][j]) + (alpha * sample);
                                 
                                 print_Q();
-                                generate_rand();
+                                iANDj_Generator();
                                 iterations++;
                                 //Goal = true;     
                 }
@@ -206,7 +206,7 @@ void print_Q()
         cout << "\n";
 }
 
-void generate_rand()
+void iANDj_Generator()
 {
         // Generate Random Pose for current state (position)
         
diff --git a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
index 4ed5d34df2d66f0c3415b5cc36b061a63ffa6709..d67d1189a874512bd9ce5baa5e5120e867418d87 100644
--- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
+++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
@@ -27,8 +27,8 @@ double Q_curr_state = Q[i][j];
 
 double reward;
 
-int R_indx_i = row - row;
-int R_indx_j = .5 * col;
+int R_indx_i = 5/*row - row*/;
+int R_indx_j = 3/*.5 * col*/;
 
 int P_indx_i = row - 2;
 int P_indx_j = col - 1;
@@ -39,7 +39,7 @@ double sample;
 
 void print_R();
 void print_Q();
-void generate_rand();
+void iANDj_Producer();
 
 int main()
 {
@@ -115,7 +115,7 @@ int main()
                 } else
                 {
                         cout << "\nGenerating random pose in grid for 1st. time!\n";
-                        generate_rand();
+                        iANDj_Producer();
                 }
                 
                 // + Reward
@@ -145,7 +145,7 @@ int main()
                         
                         counter = 0;
                         print_Q();
-                        generate_rand();
+                        iANDj_Producer();
                         iterations++;    
                 } else if (i == P_indx_i && j == P_indx_j) // - Reward => Punishment
                 {
@@ -158,7 +158,7 @@ int main()
                                 Q[i][j] = ((1 - alpha) * Q[i][j]) + (alpha * sample);
                                 
                                 print_Q();
-                                generate_rand();
+                                iANDj_Producer();
                                 iterations++;
                                 //Goal = true;     
                 }
@@ -206,7 +206,7 @@ void print_Q()
         cout << "\n";
 }
 
-void generate_rand()
+void iANDj_Producer()
 {
         // Generate Random Pose for current state (position)
         
diff --git a/MobileRobot/Machine_Learning/Practice/iANDj b/MobileRobot/Machine_Learning/Practice/iANDj
new file mode 100755
index 0000000000000000000000000000000000000000..8d9a6c4c77a9d0c73d53f74f81a235afbeaf7bcd
Binary files /dev/null and b/MobileRobot/Machine_Learning/Practice/iANDj differ
diff --git a/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eef33e1a2d2dc728592b52561a9bf14832edcf34
--- /dev/null
+++ b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp
@@ -0,0 +1,67 @@
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <ctime>
+#include <cstdlib>
+
+using namespace std;
+
+
+int i,j;
+double y,theta;
+
+ const int row = 9;
+ const int col = 9;
+
+
+
+double theta_dwn = -.6;
+double theta_up = .6;
+double y_dwn = -.3;
+double y_up = .3;
+
+
+int main()
+{
+        int sample_rate_y = col -1;
+        int sample_rate_theta = row -1;	
+        
+	double div_theta = (theta_up - theta_dwn)/sample_rate_theta; 	
+	double div_y = (y_up - y_dwn)/sample_rate_y;
+
+cout << "\n y = ? \n" ;
+        cin >> y;
+
+
+
+cout << "\n theta = ? \n" ;
+        cin >> theta;
+
+
+
+        for(int k = 0; k <= (row - 1); k++)
+        {
+                if(theta >= (theta_dwn - ((.5 + k) * div_theta)) && theta < (theta_dwn + ((.5 + k) * div_theta)))
+                {
+                        i = k;
+                        break;
+                } 
+        }
+
+        for(int l = 0; l <= (col - 1); l++)
+        {
+                if(y >= (y_dwn - ((.5 + l) * div_y)) && y < (y_dwn + ((.5 + l) * div_y)))
+                {
+                        j = l;
+                        break;
+                }
+        }
+
+cout << "\n i = " << i << " and j = " << j <<"\n";
+
+
+return 0;
+
+
+
+}
diff --git a/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp~ b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp~
new file mode 100644
index 0000000000000000000000000000000000000000..7035b08da84bf30becb43f2ceada32eb9a07dbce
--- /dev/null
+++ b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp~
@@ -0,0 +1,67 @@
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <ctime>
+#include <cstdlib>
+
+using namespace std;
+
+
+int i,j;
+double y,theta;
+
+ const int row = 7;
+ const int col = 7;
+
+
+
+double theta_dwn = -.6;
+double theta_up = .6;
+double y_dwn = -.3;
+double y_up = .3;
+
+
+int main()
+{
+        int sample_rate_y = col -1;
+        int sample_rate_theta = row -1;	
+        
+	double div_theta = (theta_up - theta_dwn)/sample_rate_theta; 	
+	double div_y = (y_up - y_dwn)/sample_rate_y;
+
+cout << "\n y = ? \n" ;
+        cin >> y;
+
+
+
+cout << "\n theta = ? \n" ;
+        cin >> theta;
+
+
+
+        for(int k = 0; k <= (row - 1); k++)
+        {
+                if(theta >= (theta_dwn - ((.5 + k) * div_theta)) && theta < (theta_dwn + ((.5 + k) * div_theta)))
+                {
+                        i = k;
+                        break;
+                } 
+        }
+
+        for(int l = 0; l <= (col - 1); l++)
+        {
+                if(y >= (y_dwn - ((.5 + l) * div_y)) && y < (y_dwn + ((.5 + l) * div_y)))
+                {
+                        j = l;
+                        break;
+                }
+        }
+
+cout << "\n i = " << i << " and j = " << j <<"\n";
+
+
+return 0;
+
+
+
+}