diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 9619df65eeb66cca77437283f92615756fe56595..e7e8fbd878f6db81b71a785ba5c4cac6a22896ec 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -121,15 +121,11 @@ public:
   void GenerateRandomVal();
   void RandomPose(double X_rand, double Y_rand, double theta_rand);
 
-         static double Pos_Px,Pos_Ix,Pos_Dx;
-	 static double Pos_Py,Pos_Iy,Pos_Dy;
+         static double Kp_x,Ki_x,Kd_x;
+	 
+	static double Kp_theta,Ki_theta,Kd_theta;
 
-         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;
 	double x_rand_SM;
@@ -163,7 +159,34 @@ public:
         static double control_signalYAW;
         // ---- CONTROLL PARAMETERS ------ //
         
-        
+	        
+	// ---- Q_LEARNING PARAMETERS ------ //
+
+	static double x_new,y_new,theta_new; 	// inp (y,theta)
+	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 int row, col;
+	static double gamma;
+	static double alpha;
+	static double R_step;
+	
+	double R[row][col];
+	double Q[row][col];
+
+	static double Q_curr_state,Q_next_state;
+	static double reward;
+	static int counter;
+	static int Time_Reward;
+	static double sample;
+
+	void Q_Learning();
+
+	// ---- Q_LEARNING PARAMETERS ------ //
+
+
 	// ---- TIMER PARAMETERS ------ //
 	static int msec,sec,min,hr;
 	// ---- TIMER PARAMETERS ------ //
@@ -181,13 +204,4 @@ public:
         double camPose[6];
         double RefPose[6];
         
-        /*double prev_error[6];
-        double int_error[6];
-        double curr_error[6];
-        double diff[6];
-        double p_term[6];
-        double d_term[6];
-        double i_term[6];
-        double control_signal[6];*/
-        
 };
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/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 18e3e50d7ff8d8bb5c5dc8b95f70694b9ce853cb..9552bb01bb0eb76d47c08f38e5308181f66deed5 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -46,7 +46,6 @@ using namespace cv;
 using namespace aruco;
 using namespace std;
 
-
 float ImageConverter::TheMarkerSize = .1; // Default marker size
 
 int ImageConverter::Thresh1_min = 10;
@@ -68,45 +67,64 @@ int ImageConverter::ThresParam1 = 0;
 int ImageConverter::ThresParam2 = 0;
 
 // ---- CONTROLL PARAMETERS ------ //
-double ImageConverter::prev_errorX;
-double ImageConverter::curr_errorX;
-double ImageConverter::int_errorX;
-double ImageConverter::diffX;
+double ImageConverter::prev_errorX, ImageConverter::curr_errorX, ImageConverter::int_errorX, ImageConverter::diffX;
+
 double ImageConverter::p_termX = 0;
 double ImageConverter::i_termX = 0;
 double ImageConverter::d_termX = 0;
-double ImageConverter::control_signalX;
 
-double ImageConverter::prev_errorY;
-double ImageConverter::curr_errorY;
-double ImageConverter::int_errorY;
-double ImageConverter::diffY;
+double ImageConverter::prev_errorY, ImageConverter::curr_errorY, ImageConverter::int_errorY, ImageConverter::diffY;
+
 double ImageConverter::p_termY = 0;
 double ImageConverter::i_termY = 0;
 double ImageConverter::d_termY = 0;
-double ImageConverter::control_signalY;
 
-double ImageConverter::prev_errorYAW;
-double ImageConverter::curr_errorYAW;
-double ImageConverter::int_errorYAW;
-double ImageConverter::diffYAW;
+double ImageConverter::prev_errorYAW,ImageConverter::curr_errorYAW,ImageConverter::int_errorYAW,ImageConverter::diffYAW;
+
 double ImageConverter::p_termYAW = 0;
 double ImageConverter::i_termYAW = 0;
 double ImageConverter::d_termYAW = 0;
-double ImageConverter::control_signalYAW;
+
+
+double ImageConverter::control_signalX, ImageConverter::control_signalY, 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;
+// ----------------  PID gains---------------- //
+double ImageConverter::Kp_y = .4;
+double ImageConverter::Ki_y = .0042;
+double ImageConverter::Kd_y = 0;
 
-double ImageConverter::Ang_P = .2 * Pos_Py;
-double ImageConverter::Ang_I = (8/15) * Pos_Iy;
-double ImageConverter::Ang_D = .2 * Pos_Dy;
+double ImageConverter::Kp_theta = .2 * Kp_y;
+double ImageConverter::Ki_theta = (8/15) * Ki_y;
+double ImageConverter::Kd_theta = .2 * Kd_y;
+// ----------------  PID gains---------------- //
 
-double ImageConverter::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration;
+// random pose initialized
+double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new;
+
+// ----------------  Q_LEARNING PARAMETERS ---------------- //
+
+double ImageConverter::gamma = .8;
+double ImageConverter::alpha = .1;
+double ImageConverter::R_step = 200;
+
+
+int ImageConverter::row = ;
+int ImageConverter::col = ;
+
+double ImageConverter::Q_curr_state = Q[y_new][theta_new];
+double ImageConverter::Q_next_state;
 
+int ImageConverter::counter = 1;
+int ImageConverter::Time_Reward;
+double ImageConverter::sample;
+
+
+
+
+// ----------------  Q_LEARNING PARAMETERS ---------------- //
+
+double ImageConverter::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration;
 double ImageConverter::zeroMax = .0000000000000000001;
 double ImageConverter::zeroMin = -.0000000000000000001;
 
@@ -124,8 +142,6 @@ double ImageConverter::x_thresh_undock = .02;
 double ImageConverter::y_thresh_undock = .015;
 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() : 
@@ -457,12 +473,12 @@ void ImageConverter::ContStart()
 
 }
 
-void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
+void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber
 {
 camPose[0] = CamFB->pose.position.x;
-camPose[1] = CamFB->pose.position.y;
+camPose[1] = CamFB->pose.position.y; // y pose
 camPose[2] = CamFB->pose.position.z;
-camPose[3] = CamFB->pose.orientation.x;
+camPose[3] = CamFB->pose.orientation.x; //  theta orientation
         
         // in Marker coordinate sys. 
         
@@ -475,7 +491,7 @@ camPose[3] = CamFB->pose.orientation.x;
         
 	
 	ROS_INFO_STREAM("--------- PID gains in trial no. " << docking_counter << " : ---------\n");
-	ROS_INFO_STREAM(" Kp = " << Pos_Py << " ,  Ki = " << Pos_Iy << " , Kd = " << Pos_Dy << "\n");
+	ROS_INFO_STREAM(" Kp = " << Kp_y << " ,  Ki = " << Ki_y << " , Kd = " << Kd_y << "\n");
         
 	ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
 	ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs X_ref = " << RefPose[2] << " \n");
@@ -487,7 +503,7 @@ camPose[3] = CamFB->pose.orientation.x;
 
 	if(Go2RandomPose == false)
 	{
-		ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM ... ---------  \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
@@ -528,7 +544,7 @@ camPose[3] = CamFB->pose.orientation.x;
 		}
 	} else
 	{
-  		ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ... ---------\n");		
+  		ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ---------\n");		
 		RandomPose(x_new,y_new,theta_new);
 	}
 }
@@ -582,9 +598,9 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         	diffY = ((curr_errorY - prev_errorY) / dt);
         
         	// scalling
-        	p_termY = Pos_Py * curr_errorY;
-        	i_termY = Pos_Iy * int_errorY;
-        	d_termY = Pos_Dy * diffY;
+        	p_termY = Kp_y * curr_errorY;
+        	i_termY = Ki_y * int_errorY;
+        	d_termY = Kd_y * diffY;
 
 		//ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n");	      
         	// control signal
@@ -622,9 +638,9 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
 
 		ROS_INFO_STREAM(" adjusting orientation! \n");	        		
 		// scalling
-        	p_termYAW = Ang_P * curr_errorYAW;
-       		i_termYAW = Ang_I * int_errorYAW;
-       		d_termYAW = Ang_D * diffYAW;
+        	p_termYAW = Kp_theta * curr_errorYAW;
+       		i_termYAW = Ki_theta * int_errorYAW;
+       		d_termYAW = Kd_theta * diffYAW;
         	
 		// control signal
         	control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
@@ -704,21 +720,19 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega
 void ImageConverter::GenerateRandomVal()
 {
 	// ---------------- PID gains ------------------
-	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
+	Kp_y = ((double) rand() / (RAND_MAX)) * (.76 - .4) + .4; 	//.1 < Kp < .76
+	Ki_y = ((double) rand() / (RAND_MAX)) * .006;			// 0 < Ki < .006
+	Kd_y = ((double) rand() / (RAND_MAX)) * .02;			// 0 < Kd < .01
 	
 	// ------------------ Generating Random Pose ------------------
 	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]);
+	y_new = ((double) rand() / (RAND_MAX)) * (.48 - (-.24)) + (-.24); // will be used for Q_Learning
+	theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]); // will be used for Q_Learning
 }
 
 void ImageConverter::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(" 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;
@@ -819,3 +833,109 @@ geometry_msgs::Twist msg_new;
 commandPub.publish(msg_new);
 	
 }
+
+void ImageConverter::Q_Learning()
+{
+	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;
+          }
+}
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index d8d96b0d715670d0e836a7e8f2706134ac9aa7cf..1ff2e063026d15cfdf3f2512f0527e093e3fa7d7 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -46,7 +46,6 @@ using namespace cv;
 using namespace aruco;
 using namespace std;
 
-
 float ImageConverter::TheMarkerSize = .1; // Default marker size
 
 int ImageConverter::Thresh1_min = 10;
@@ -68,75 +67,81 @@ int ImageConverter::ThresParam1 = 0;
 int ImageConverter::ThresParam2 = 0;
 
 // ---- CONTROLL PARAMETERS ------ //
-double ImageConverter::prev_errorX;
-double ImageConverter::curr_errorX;
-double ImageConverter::int_errorX;
-double ImageConverter::diffX;
+double ImageConverter::prev_errorX, ImageConverter::curr_errorX, ImageConverter::int_errorX, ImageConverter::diffX;
+
 double ImageConverter::p_termX = 0;
 double ImageConverter::i_termX = 0;
 double ImageConverter::d_termX = 0;
-double ImageConverter::control_signalX;
 
-double ImageConverter::prev_errorY;
-double ImageConverter::curr_errorY;
-double ImageConverter::int_errorY;
-double ImageConverter::diffY;
+double ImageConverter::prev_errorY, ImageConverter::curr_errorY, ImageConverter::int_errorY, ImageConverter::diffY;
+
 double ImageConverter::p_termY = 0;
 double ImageConverter::i_termY = 0;
 double ImageConverter::d_termY = 0;
-double ImageConverter::control_signalY;
 
-double ImageConverter::prev_errorYAW;
-double ImageConverter::curr_errorYAW;
-double ImageConverter::int_errorYAW;
-double ImageConverter::diffYAW;
+double ImageConverter::prev_errorYAW,ImageConverter::curr_errorYAW,ImageConverter::int_errorYAW,ImageConverter::diffYAW;
+
 double ImageConverter::p_termYAW = 0;
 double ImageConverter::i_termYAW = 0;
 double ImageConverter::d_termYAW = 0;
-double ImageConverter::control_signalYAW;
+
+
+double ImageConverter::control_signalX, ImageConverter::control_signalY, ImageConverter::control_signalYAW;
 // ---- CONTROLL PARAMETERS ------ //
 
-// ----------------  Initial Gains which dock the robot successfully ---------------- //
+// ----------------  PID gains---------------- //
+double ImageConverter::Kp_y = .4;
+double ImageConverter::Ki_y = .0042;
+double ImageConverter::Kd_y = 0;
 
-double ImageConverter::Pos_Px = .1;
-double ImageConverter::Pos_Ix = 0.0028;
-double ImageConverter::Pos_Dx = 0;
+double ImageConverter::Kp_theta = .2 * Kp_y;
+double ImageConverter::Ki_theta = (8/15) * Ki_y;
+double ImageConverter::Kd_theta = .2 * Kd_y;
+// ----------------  PID gains---------------- //
 
-double ImageConverter::Pos_Py = 4 * Pos_Px;
-double ImageConverter::Pos_Iy = 1.5 * Pos_Ix;
-double ImageConverter::Pos_Dy = 4 * Pos_Dx;
+// random pose initialized
+double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new;
 
-/*double ImageConverter::S_Ang_P = .2 * Pos_Px;
-double ImageConverter::S_Ang_I = .2 * Pos_Ix;
-double ImageConverter::S_Ang_D = .2 * Pos_Dx;
+// ----------------  Q_LEARNING PARAMETERS ---------------- //
 
-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::gamma = .8;
+double ImageConverter::alpha = .1;
+double ImageConverter::R_step = 200;
+
+
+int ImageConverter::row = ;
+int ImageConverter::col = ;
+
+double ImageConverter::Q_curr_state = Q[y_new][theta_new];
+double ImageConverter::Q_next_state;
+
+int ImageConverter::counter = 1;
+int ImageConverter::Time_Reward;
+double ImageConverter::sample;
 
-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;
 
+
+// ----------------  Q_LEARNING PARAMETERS ---------------- //
+
+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
-double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new;
 
 double ImageConverter::docking_counter = 1;
  ImageConverter::ImageConverter() : 
@@ -151,8 +156,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
@@ -468,13 +473,13 @@ void ImageConverter::ContStart()
 
 }
 
-void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
+void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber
 {
 camPose[0] = CamFB->pose.position.x;
-camPose[1] = CamFB->pose.position.y;
+camPose[1] = CamFB->pose.position.y; // y pose
 camPose[2] = CamFB->pose.position.z;
-camPose[3] = CamFB->pose.orientation.x;
-
+camPose[3] = CamFB->pose.orientation.x; //  theta orientation
+        
         // in Marker coordinate sys. 
         
         // z => X robot (thrust)
@@ -486,7 +491,7 @@ camPose[3] = CamFB->pose.orientation.x;
         
 	
 	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 = " << Kp_y << " ,  Ki = " << Ki_y << " , Kd = " << Kd_y << "\n");
         
 	ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
 	ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs X_ref = " << RefPose[2] << " \n");
@@ -498,7 +503,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
@@ -511,6 +516,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...
@@ -529,6 +535,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
@@ -537,7 +544,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);
 	}
 }
@@ -545,41 +552,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)
@@ -600,11 +598,11 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         	diffY = ((curr_errorY - prev_errorY) / dt);
         
         	// scalling
-        	p_termY = Pos_Py * curr_errorY;
-        	i_termY = Pos_Iy * int_errorY;
-        	d_termY = Pos_Dy * diffY;
+        	p_termY = Kp_y * curr_errorY;
+        	i_termY = Ki_y * int_errorY;
+        	d_termY = Kd_y * 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;
         	
@@ -637,12 +635,12 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         
         	// differentiation
         	diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
-        	
+
 		ROS_INFO_STREAM(" adjusting orientation! \n");	        		
 		// scalling
-        	p_termYAW = Ang_P * curr_errorYAW;
-       		i_termYAW = Ang_I * int_errorYAW;
-       		d_termYAW = Ang_D * diffYAW;
+        	p_termYAW = Kp_theta * curr_errorYAW;
+       		i_termYAW = Ki_theta * int_errorYAW;
+       		d_termYAW = Kd_theta * diffYAW;
         	
 		// control signal
         	control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
@@ -669,7 +667,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)
@@ -721,21 +720,19 @@ 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
+	Kp_y = ((double) rand() / (RAND_MAX)) * (.76 - .4) + .4; 	//.1 < Kp < .76
+	Ki_y = ((double) rand() / (RAND_MAX)) * .006;			// 0 < Ki < .006
+	Kd_y = ((double) rand() / (RAND_MAX)) * .02;			// 0 < Kd < .01
 	
 	// ------------------ Generating Random Pose ------------------
 	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]);
+	y_new = ((double) rand() / (RAND_MAX)) * (.48 - (-.24)) + (-.24); // will be used for Q_Learning
+	theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]); // will be used for Q_Learning
 }
 
 void ImageConverter::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(" 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;
@@ -823,31 +820,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;
@@ -855,3 +833,109 @@ geometry_msgs::Twist msg_new;
 commandPub.publish(msg_new);
 	
 }
+
+void ImageConverter::Q_Learning()
+{
+	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 = 1;  
+                } 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;
+          }
+}
diff --git a/MobileRobot/Machine_Learning/Practice/Q_Learning b/MobileRobot/Machine_Learning/Practice/Q_Learning
index 795da90ad5a7bb58acac57b42c0af4e805d3ab43..a2326691693b50e35e9bc15e880392c7235af958 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 d62ee1be52e512ab9d4b0f205d7d88143fa0bb54..1ce7497f7e7fdf2357bd36842841e381956f3575 100644
--- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
+++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
@@ -7,23 +7,20 @@
 using namespace std;
 
 
-const int row = 13;
+const int row = 48;
 const int col = 13;
 
 
 double gamma = .8;
 double alpha = .1;
-double R_step = 200;
+double R_step = 500;
 
 double R[row][col] = {0};
 double Q[row][col] = {0};
- 
-bool Goal = false;
 
 int iterations = 1;
 int it_;
 int user_action;
-int update_final_state;
 double Q_next_state;
 
 int i,j;
@@ -37,9 +34,10 @@ int R_indx_j = .5 * col;
 int P_indx_i = row - 2;
 int P_indx_j = col - 1;
 
-int counter = 1;
+int counter;
 int Time_Reward;
 double sample;
+
 void print_R();
 void print_Q();
 void generate_rand();
@@ -47,7 +45,7 @@ void generate_rand();
 int main()
 {
         R[R_indx_i][R_indx_j] = 50; // reward
-        R[P_indx_i][P_indx_j] = -100; // punishment
+        R[P_indx_i][P_indx_j] = -60; // punishment
 
         print_R();
         
@@ -146,7 +144,7 @@ int main()
                                 Q[i][j] = ((1 - alpha) * Q[i][j]) + (alpha * sample);
                         }
                         
-                        counter = 1;
+                        counter = 0;
                         print_Q();
                         generate_rand();
                         iterations++;    
@@ -167,10 +165,9 @@ int main()
                 }
                 
                 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;
+                user_action = ((double) rand() / (RAND_MAX)) * (5 - 1) + 1; // Generating random user_action  
                 printf(" ramdom user action = %i \n",user_action);
                 //cin >> user_action;
-                
           }
 return 0;
 }
@@ -212,7 +209,8 @@ void print_Q()
 
 void generate_rand()
 {
-
+        // Generate Random Pose for current state (position)
+        
         i = ((double) rand() / (RAND_MAX)) * (row) ;
         j = ((double) rand() / (RAND_MAX)) * (col) ;
         
diff --git a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
index 62b41767f5e1c8215b801d54d4e487e73aad6ee7..f3d3f7186fff41a7ae4be5a34537346741c5b6e2 100644
--- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
+++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
@@ -7,7 +7,7 @@
 using namespace std;
 
 
-const int row = 13;
+const int row = 48;
 const int col = 13;
 
 
@@ -17,13 +17,10 @@ double R_step = 200;
 
 double R[row][col] = {0};
 double Q[row][col] = {0};
- 
-bool Goal = false;
 
 int iterations = 1;
 int it_;
 int user_action;
-int update_final_state;
 double Q_next_state;
 
 int i,j;
@@ -37,9 +34,10 @@ int R_indx_j = .5 * col;
 int P_indx_i = row - 2;
 int P_indx_j = col - 1;
 
-int counter = 1;
+int counter;
 int Time_Reward;
 double sample;
+
 void print_R();
 void print_Q();
 void generate_rand();
@@ -47,7 +45,7 @@ void generate_rand();
 int main()
 {
         R[R_indx_i][R_indx_j] = 50; // reward
-        R[P_indx_i][P_indx_j] = -100; // punishment
+        R[P_indx_i][P_indx_j] = -60; // punishment
 
         print_R();
         
@@ -146,12 +144,10 @@ int main()
                                 Q[i][j] = ((1 - alpha) * Q[i][j]) + (alpha * sample);
                         }
                         
-                        counter = 1;
+                        counter = 0;
                         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";
@@ -169,10 +165,9 @@ int main()
                 }
                 
                 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;
+                user_action = ((double) rand() / (RAND_MAX)) * (5 - 1) + 1; // Generating random user_action  
                 printf(" ramdom user action = %i \n",user_action);
                 //cin >> user_action;
-                
           }
 return 0;
 }
@@ -214,7 +209,8 @@ void print_Q()
 
 void generate_rand()
 {
-
+        // Generate Random Pose for current state (position)
+        
         i = ((double) rand() / (RAND_MAX)) * (row) ;
         j = ((double) rand() / (RAND_MAX)) * (col) ;