diff --git a/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/CXX.includecache b/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/CXX.includecache
index b514746d669fc7dc69298962db3a6c2d4d59298c..9d5c4b4f54e39af0a4c6395865fab0378e5c7641 100644
--- a/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/CXX.includecache
+++ b/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/CXX.includecache
@@ -130,12 +130,6 @@ opencv2/contrib/contrib.hpp
 VisionControl.h
 -
 
-/home/faridalijani/thesis/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp
-iostream
--
-VisionControl.h
--
-
 /opt/ros/hydro/include/XmlRpcDecl.h
 ros/macros.h
 -
diff --git a/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/src/VisionControl.cpp.o b/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/src/VisionControl.cpp.o
index dc892a180231eb55831b43fe8871cf7d626ae69e..ec1763a300aa1b59cdb5b4fe50bd3db51a160c23 100644
Binary files a/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/src/VisionControl.cpp.o and b/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/src/VisionControl.cpp.o differ
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 324817a0dfc4e284e8cfb7f338c33da4e213cb80..befd277b2d1e11ed573249e070e69cc769afbbea 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -93,11 +93,10 @@ private:
         
         static const string trackbarWindowName;
         
-        void Controller(double Reference, double CamPose, double dt, int index);
-        //void Controller(double Reference[6], double RobPose[6], double dt);
+        void Controller(double RefX, double MarPoseX, double refY, double MarPoseY, double refYAW, double MarPoseYAW, double dt);
+        
+        void dock(double VelX, double VelY, double omegaZ);
         
-        void dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW);
-        //void dock(double speed[6]);
 public:
 
   ImageConverter();
@@ -119,26 +118,41 @@ public:
   void myhandler(int value);
   
   
-        static double prev_error;
-        
-        static double int_error;
-        
-        static double curr_error;
-        
-        static double diff;
-        
-        static double p_term;
-        
-        static double d_term;
-        
-        static double i_term;
-        
-        static double control_signal;
-        
-        
+  
         static double prop_gain;
         static double integ_gain;
         static double deriv_gain;
+        
+        // ---- CONTROLL PARAMETERS ------ //
+        static double prev_errorX;
+        static double int_errorX;
+        static double curr_errorX;
+        static double diffX;
+        static double p_termX;
+        static double d_termX;
+        static double i_termX;
+        static double control_signalX;
+        
+        static double prev_errorY;
+        static double int_errorY;
+        static double curr_errorY;
+        static double diffY;
+        static double p_termY;
+        static double d_termY;
+        static double i_termY;
+        static double control_signalY;
+        
+        static double prev_errorYAW;
+        static double int_errorYAW;
+        static double curr_errorYAW;
+        static double diffYAW;
+        static double p_termYAW;
+        static double d_termYAW;
+        static double i_termYAW;
+        static double control_signalYAW;
+        // ---- CONTROLL PARAMETERS ------ //
+        
+        
         static double zeroMin,zeroMax,eps;
   
         double marpose[6];
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index 63c911ab444207345bc4ce9da9bb89e557bfa49e..ee062152358fbb77bdcff65de6bef288e88d0496 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -87,16 +87,15 @@ private:
         static float TheMarkerSize;
         
         void ContStart();
-        static bool update_images,MarKerPoseObtained,found,seekingMarker;
+        static bool update_images,found;
         
         bool readArguments ( int argc,char **argv );
         
         static const string trackbarWindowName;
         
-        void Controller(double Reference, double CamPose, double dt, int index);
-        //void Controller(double Reference[6], double RobPose[6], double dt);
+        void Controller(double RefX, double MarPoseX, double refY, double MarPoseY, double refYAW, double MarPoseYAW, double dt);
         
-        void dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW);
+        void dock(double VelX, double VelY, double omegaZ);
         //void dock(double speed[6]);
 public:
 
@@ -119,26 +118,41 @@ public:
   void myhandler(int value);
   
   
-        static double prev_error;
-        
-        static double int_error;
-        
-        static double curr_error;
-        
-        static double diff;
-        
-        static double p_term;
-        
-        static double d_term;
-        
-        static double i_term;
-        
-        static double control_signal;
-        
-        
+  
         static double prop_gain;
         static double integ_gain;
         static double deriv_gain;
+        
+        // ---- CONTROLL PARAMETERS ------ //
+        static double prev_errorX;
+        static double int_errorX;
+        static double curr_errorX;
+        static double diffX;
+        static double p_termX;
+        static double d_termX;
+        static double i_termX;
+        static double control_signalX;
+        
+        static double prev_errorY;
+        static double int_errorY;
+        static double curr_errorY;
+        static double diffY;
+        static double p_termY;
+        static double d_termY;
+        static double i_termY;
+        static double control_signalY;
+        
+        static double prev_errorYAW;
+        static double int_errorYAW;
+        static double curr_errorYAW;
+        static double diffYAW;
+        static double p_termYAW;
+        static double d_termYAW;
+        static double i_termYAW;
+        static double control_signalYAW;
+        // ---- CONTROLL PARAMETERS ------ //
+        
+        
         static double zeroMin,zeroMax,eps;
   
         double marpose[6];
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 31ca1847bb69f18f260f6400e2378abd6d06529a..d3fb9faacecd172f053a28695759ef342cc0023a 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -43,6 +43,10 @@ using namespace cv;
 using namespace aruco;
 using namespace std;
 
+double ImageConverter::prop_gain = .01;
+double ImageConverter::integ_gain = .5;
+double ImageConverter::deriv_gain = 0;
+
 float ImageConverter::TheMarkerSize = -1;
 
 int ImageConverter::Thresh1_min = 5;
@@ -62,20 +66,34 @@ bool ImageConverter::found = false;
 int ImageConverter::ThresParam1 = 0;
 int ImageConverter::ThresParam2 = 0;
 
-
-double ImageConverter::prev_error;
-double ImageConverter::curr_error;
-double ImageConverter::int_error;
-double ImageConverter::diff;
-
-double ImageConverter::p_term = 0;
-double ImageConverter::i_term = 0;
-double ImageConverter::d_term = 0;
-double ImageConverter::control_signal;
-
-double ImageConverter::prop_gain = 5;
-double ImageConverter::integ_gain = 3;
-double ImageConverter::deriv_gain = 0;
+// ---- CONTROLL PARAMETERS ------ //
+double ImageConverter::prev_errorX;
+double ImageConverter::curr_errorX;
+double ImageConverter::int_errorX;
+double 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::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::p_termYAW = 0;
+double ImageConverter::i_termYAW = 0;
+double ImageConverter::d_termYAW = 0;
+double ImageConverter::control_signalYAW;
+// ---- CONTROLL PARAMETERS ------ //
 
 double ImageConverter::zeroMax = .0000000000000000001;
 double ImageConverter::zeroMin = -.0000000000000000001;
@@ -210,9 +228,9 @@ void ImageConverter::ProgStart(int argc,char** argv)
         const std::string vsa = "http://192.168.0.101:8080/video?x.mjpeg";
 
         // -- publishing video stream with Android Camera--
-        TheVideoCapturer.open(vsa);
+        //TheVideoCapturer.open(vsa);
 
-	//TheVideoCapturer.open(0);
+	TheVideoCapturer.open(1);
 	
 	// Check video is open
 	if (!TheVideoCapturer.isOpened())
@@ -409,85 +427,61 @@ void ImageConverter::ContStart()
 void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
 {
        
-RefPose[0] = -.1;
-RefPose[1] = .01;
+RefPose[0] = -.0957;
+RefPose[1] = .00740;
 RefPose[2] = .35;
-RefPose[3] = 75.5;
+RefPose[3] = -.68952;
  
-        ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n");
-        ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n");
+        //ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n");
+        //ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n");
         
-        ROS_INFO_STREAM(" Ymar = " << CamFB->pose.position.y << " m. \n");
-        ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n");
+        
+        // in Marker coordinate sys. 
+        
+        // z => X robot (thrust)
+        // y => -Y robot (left - right)
+        // x =>  Z robot (NOT applicabale in our case!)
+        
+        // correspondingly ... 
+        // roll in Marker coordinate => yaw in Robot coordinate
         
         ROS_INFO_STREAM(" Zmar = " << CamFB->pose.position.z << " m. \n");
         ROS_INFO_STREAM(" Zref = " << RefPose[2] << " m. \n");
         
+        ROS_INFO_STREAM(" Ymar = " << CamFB->pose.position.y << " m. \n");
+        ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n");
+        
         ROS_INFO_STREAM(" rollmar = " << CamFB->pose.orientation.x << " rad. \n");
         ROS_INFO_STREAM(" rollref = " << RefPose[3] << " rad. \n");
         
         ROS_INFO_STREAM(" ------------------------------------------------------------- \n");
     
         
-        if (/*(abs(RefPose[0] - CamFB->pose.position.x) <= eps) &&*/
+        if (
             (abs(RefPose[1] - CamFB->pose.position.y) <= eps) && // Y
-            (abs(RefPose[2] - CamFB->pose.position.z) <= eps))  //  Z
+            (abs(RefPose[2] - CamFB->pose.position.z) <= eps) && // Z
+            (abs(RefPose[3] - CamFB->pose.orientation.x) <= eps) // Yaw
+            )
         {
-                if (((RefPose[3] - CamFB->pose.orientation.x) <= eps) /*&& // Yaw
-                    ((RefPose[4] - CamFB->pose.orientation.y) <= eps) &&
-                    ((RefPose[5] - CamFB->pose.orientation.z) <= eps) &&
-                    ((RefPose[6] - CamFB->pose.orientation.w) <= eps)*/)
-                {
+                
                         ROS_INFO("------------------------ Dock is completed ! ---------------------- \n "); 
                         keepMoving = false;
-                        
-                }else if (abs(RefPose[3] - CamFB->pose.orientation.x) > eps)
-                {
-                        ROS_INFO(" Calculating control signal For ROLL! \n ");
-                        Controller(RefPose[3],CamFB->pose.orientation.x,.01,6);
-                
-                }/* else if (abs(RefPose[4] - CamFB->pose.orientation.y) > eps)
-                {
-                        ROS_INFO(" Calculating control signal For PITCH! \n ");
-                        Controller(RefPose[4],CamFB->pose.orientation.y,.01,5);
-                
-                } else if (abs(RefPose[5] - CamFB->pose.orientation.z) > eps)
-                {
-                        ROS_INFO(" Calculating control signal For YAW! \n ");
-                        Controller(RefPose[5],CamFB->pose.orientation.z,.01,6);
-                        
-                } else if (abs(RefPose[6] - CamFB->pose.orientation.w > eps)
-                {
-                        ROS_INFO(" Calculating control signal For W! \n ");
-                        Controller(RefPose[6],CamFB->pose.orientation.w,.01,7);
-                }*/             
-                       
-                
-        } /*else if (abs(RefPose[0] - CamFB->pose.position.x) > eps)
+                 
+        } else
         {
-                ROS_INFO(" Calculating control signal For X-direction! \n ");
-                Controller(RefPose[0],CamFB->pose.position.x,.01,1);
-                
-        } */else if (abs(RefPose[1] - CamFB->pose.position.y) > eps)
-        {
-                ROS_INFO(" Calculating control signal For Y-direction! \n ");
-                Controller(RefPose[1],CamFB->pose.position.y,.01,2);
-                
-        } else if (abs(RefPose[2] - CamFB->pose.position.z) > eps)
-        {
-                ROS_INFO(" Calculating control signal For Z-direction! \n ");
-                Controller(RefPose[2],CamFB->pose.position.z,.01,1);
+                ROS_INFO(" Calculating control signal ! \n ");
+                Controller(RefPose[2], CamFB->pose.position.z, RefPose[1], CamFB->pose.position.y, RefPose[3], CamFB->pose.orientation.x,.01);
         }
 }
 
-//void ImageConverter::Controller(double Reference[i], double RobPose[i], double dt)
-void ImageConverter::Controller(double Reference, double CamPose, double dt, int index)
+void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
 {
+        // -----------------X--------------------- //
         // e(t) = setpoint - actual value;
-        curr_error = Reference - CamPose;
+        curr_errorX = RefX - MarPoseX;
         
         // Integrated error
-        int_error +=  curr_error * dt;
+        int_errorX +=  curr_errorX * dt;
         /*
         // -- windup gaurd -- 
         if (int_error < )
@@ -496,66 +490,87 @@ void ImageConverter::Controller(double Reference, double CamPose, double dt, int
         {}*/
         
         // differentiation
-        diff = ((curr_error - prev_error) / dt);
+        diffX = ((curr_errorX - prev_errorX) / dt);
         
         // scalling
-        p_term = prop_gain  * curr_error;
-        i_term = integ_gain * int_error;
-        d_term = deriv_gain * diff;
+        p_termX = prop_gain  * curr_errorX;
+        i_termX = integ_gain * int_errorX;
+        d_termX = deriv_gain * diffX;
         
         // control signal
-        control_signal = p_term + i_term + d_term;
+        control_signalX = p_termX + i_termX + d_termX;
         
         
         // save the current error as the previous one
         // for the next iteration.
-        prev_error = curr_error;        
+        prev_errorX = curr_errorX;        
         
-        //dock(control_signal);
+        // -----------------Y--------------------- // 
+        // e(t) = setpoint - actual value;
+        curr_errorY = RefY - MarPoseY;
         
-        if (index == 1)
-        {
-                dock(control_signal, 0, 0, 0, 0, 0, 0);
-        } else if (index == 2)
-        {
-                dock(0, -control_signal, 0, 0, 0, 0, 0);
-        } else if (index == 3)
-        {
-                dock(0, 0, control_signal, 0, 0, 0, 0);
-        }else if (index == 4)
-        {
-                dock(0, 0, 0, control_signal, 0, 0, 0);
-        } else if (index == 5)
-        {
-                dock(0, 0, 0, 0, control_signal, 0, 0);
-        } else if (index == 6)
-        {
-                dock(0, 0, 0, 0, 0, control_signal, 0);
-        } /*else if (index == 7)
-        {
-                dock(0, 0, 0, 0, 0, 0, control_signal);
-        }*/
+        // Integrated error
+        int_errorY +=  curr_errorY * dt;
+        /*
+        // -- windup gaurd -- 
+        if (int_error < )
+        {}
+        else if ()
+        {}*/
+        
+        // differentiation
+        diffY = ((curr_errorY - prev_errorY) / dt);
+        
+        // scalling
+        p_termY = prop_gain  * curr_errorY;
+        i_termY = integ_gain * int_errorY;
+        d_termY = deriv_gain * diffY;
+        
+        // control signal
+        control_signalY = p_termY + i_termY + d_termY;
+        
+        
+        // save the current error as the previous one
+        // for the next iteration.
+        prev_errorY = curr_errorY;  
+        
+        // -------------------YAW--------------------------//
+        // e(t) = setpoint - actual value;
+        curr_errorYAW = RefYAW - MarPoseYAW;
+        
+        // Integrated error
+        int_errorYAW +=  curr_errorYAW * dt;
+        /*
+        // -- windup gaurd -- 
+        if (int_error < )
+        {}
+        else if ()
+        {}*/
+        
+        // differentiation
+        diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
+        
+        // scalling
+        p_termYAW = prop_gain  * curr_errorYAW;
+        i_termYAW = integ_gain * int_errorYAW;
+        d_termYAW = deriv_gain * diffYAW;
+        
+        // control signal
+        control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
+        
+        
+        // save the current error as the previous one
+        // for the next iteration.
+        prev_errorYAW = curr_errorYAW;  
+        
+        dock(-control_signalX, -control_signalY, -control_signalYAW);
 }
-
-//void ImageConverter::dock(double speed[6])
-void ImageConverter::dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW)
+void ImageConverter::dock(double VelX, double VelY, double omegaZ)
 {
         geometry_msgs::Twist msg;
         
-	/*msg.linear.x = speed[0];
-	msg.linear.y = speed[1];
-	msg.linear.z = speed[2];
-	
-	msg.angular.x = speed[3];
-	msg.angular.y = speed[4];
-	msg.angular.z = speed[5];*/
-	
 	msg.linear.x = VelX;
 	msg.linear.y = VelY;
-	msg.linear.z = VelZ;
-	
-	msg.angular.x = omegaX;
-	msg.angular.y = omegaY;
 	msg.angular.z = omegaZ;
 	
 	commandPub.publish(msg);
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 47de5d7146659798a1b6f7bff084fccdb6a090fb..1868461d136e8ae7c4b09b3fe0a03a80b758c3bb 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -43,6 +43,10 @@ using namespace cv;
 using namespace aruco;
 using namespace std;
 
+double ImageConverter::prop_gain = .01;
+double ImageConverter::integ_gain = .5;
+double ImageConverter::deriv_gain = 0;
+
 float ImageConverter::TheMarkerSize = -1;
 
 int ImageConverter::Thresh1_min = 5;
@@ -62,20 +66,34 @@ bool ImageConverter::found = false;
 int ImageConverter::ThresParam1 = 0;
 int ImageConverter::ThresParam2 = 0;
 
-
-double ImageConverter::prev_error;
-double ImageConverter::curr_error;
-double ImageConverter::int_error;
-double ImageConverter::diff;
-
-double ImageConverter::p_term = 0;
-double ImageConverter::i_term = 0;
-double ImageConverter::d_term = 0;
-double ImageConverter::control_signal;
-
-double ImageConverter::prop_gain = 15;
-double ImageConverter::integ_gain = 3;
-double ImageConverter::deriv_gain = 0;
+// ---- CONTROLL PARAMETERS ------ //
+double ImageConverter::prev_errorX;
+double ImageConverter::curr_errorX;
+double ImageConverter::int_errorX;
+double 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::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::p_termYAW = 0;
+double ImageConverter::i_termYAW = 0;
+double ImageConverter::d_termYAW = 0;
+double ImageConverter::control_signalYAW;
+// ---- CONTROLL PARAMETERS ------ //
 
 double ImageConverter::zeroMax = .0000000000000000001;
 double ImageConverter::zeroMin = -.0000000000000000001;
@@ -409,85 +427,61 @@ void ImageConverter::ContStart()
 void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
 {
        
-RefPose[0] = -.1;
-RefPose[1] = .01;
+RefPose[0] = -.0957;
+RefPose[1] = .00740;
 RefPose[2] = .35;
-RefPose[3] = 75.5;
+RefPose[3] = -.68952;
  
-        ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n");
-        ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n");
+        //ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n");
+        //ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n");
         
-        ROS_INFO_STREAM(" Ymar = " << CamFB->pose.position.y << " m. \n");
-        ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n");
+        
+        // in Marker coordinate sys. 
+        
+        // z => X robot (thrust)
+        // y => -Y robot (left - right)
+        // x =>  Z robot (NOT applicabale in our case!)
+        
+        // correspondingly ... 
+        // roll in Marker coordinate => yaw in Robot coordinate
         
         ROS_INFO_STREAM(" Zmar = " << CamFB->pose.position.z << " m. \n");
         ROS_INFO_STREAM(" Zref = " << RefPose[2] << " m. \n");
         
+        ROS_INFO_STREAM(" Ymar = " << CamFB->pose.position.y << " m. \n");
+        ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n");
+        
         ROS_INFO_STREAM(" rollmar = " << CamFB->pose.orientation.x << " rad. \n");
         ROS_INFO_STREAM(" rollref = " << RefPose[3] << " rad. \n");
         
         ROS_INFO_STREAM(" ------------------------------------------------------------- \n");
     
         
-        if (/*(abs(RefPose[0] - CamFB->pose.position.x) <= eps) &&*/
+        if (
             (abs(RefPose[1] - CamFB->pose.position.y) <= eps) && // Y
-            (abs(RefPose[2] - CamFB->pose.position.z) <= eps))  //  Z
+            (abs(RefPose[2] - CamFB->pose.position.z) <= eps) && // Z
+            (abs(RefPose[3] - CamFB->pose.orientation.x) <= eps) // Yaw
+            )
         {
-                if (((RefPose[3] - CamFB->pose.orientation.x) <= eps) /*&& // Yaw
-                    ((RefPose[4] - CamFB->pose.orientation.y) <= eps) &&
-                    ((RefPose[5] - CamFB->pose.orientation.z) <= eps) &&
-                    ((RefPose[6] - CamFB->pose.orientation.w) <= eps)*/)
-                {
+                
                         ROS_INFO("------------------------ Dock is completed ! ---------------------- \n "); 
                         keepMoving = false;
-                        
-                }else if (abs(RefPose[3] - CamFB->pose.orientation.x) > eps)
-                {
-                        ROS_INFO(" Calculating control signal For ROLL! \n ");
-                        Controller(RefPose[3],CamFB->pose.orientation.x,.01,6);
-                
-                }/* else if (abs(RefPose[4] - CamFB->pose.orientation.y) > eps)
-                {
-                        ROS_INFO(" Calculating control signal For PITCH! \n ");
-                        Controller(RefPose[4],CamFB->pose.orientation.y,.01,5);
-                
-                } else if (abs(RefPose[5] - CamFB->pose.orientation.z) > eps)
-                {
-                        ROS_INFO(" Calculating control signal For YAW! \n ");
-                        Controller(RefPose[5],CamFB->pose.orientation.z,.01,6);
-                        
-                } else if (abs(RefPose[6] - CamFB->pose.orientation.w > eps)
-                {
-                        ROS_INFO(" Calculating control signal For W! \n ");
-                        Controller(RefPose[6],CamFB->pose.orientation.w,.01,7);
-                }*/             
-                       
-                
-        } /*else if (abs(RefPose[0] - CamFB->pose.position.x) > eps)
+                 
+        } else
         {
-                ROS_INFO(" Calculating control signal For X-direction! \n ");
-                Controller(RefPose[0],CamFB->pose.position.x,.01,1);
-                
-        } */else if (abs(RefPose[1] - CamFB->pose.position.y) > eps)
-        {
-                ROS_INFO(" Calculating control signal For Y-direction! \n ");
-                Controller(RefPose[1],CamFB->pose.position.y,.01,2);
-                
-        } else if (abs(RefPose[2] - CamFB->pose.position.z) > eps)
-        {
-                ROS_INFO(" Calculating control signal For Z-direction! \n ");
-                Controller(RefPose[2],CamFB->pose.position.z,.01,1);
+                ROS_INFO(" Calculating control signal ! \n ");
+                Controller(RefPose[2], CamFB->pose.position.z, RefPose[1], CamFB->pose.position.y, RefPose[3], CamFB->pose.orientation.x,.01);
         }
 }
 
-//void ImageConverter::Controller(double Reference[i], double RobPose[i], double dt)
-void ImageConverter::Controller(double Reference, double CamPose, double dt, int index)
+void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
 {
+        // -----------------X--------------------- //
         // e(t) = setpoint - actual value;
-        curr_error = Reference - CamPose;
+        curr_errorX = RefX - MarPoseX;
         
         // Integrated error
-        int_error +=  curr_error * dt;
+        int_errorX +=  curr_errorX * dt;
         /*
         // -- windup gaurd -- 
         if (int_error < )
@@ -496,66 +490,87 @@ void ImageConverter::Controller(double Reference, double CamPose, double dt, int
         {}*/
         
         // differentiation
-        diff = ((curr_error - prev_error) / dt);
+        diffX = ((curr_errorX - prev_errorX) / dt);
         
         // scalling
-        p_term = prop_gain  * curr_error;
-        i_term = integ_gain * int_error;
-        d_term = deriv_gain * diff;
+        p_termX = prop_gain  * curr_errorX;
+        i_termX = integ_gain * int_errorX;
+        d_termX = deriv_gain * diffX;
         
         // control signal
-        control_signal = p_term + i_term + d_term;
+        control_signalX = p_termX + i_termX + d_termX;
         
         
         // save the current error as the previous one
         // for the next iteration.
-        prev_error = curr_error;        
+        prev_errorX = curr_errorX;        
         
-        //dock(control_signal);
+        // -----------------Y--------------------- // 
+        // e(t) = setpoint - actual value;
+        curr_errorY = RefY - MarPoseY;
         
-        if (index == 1)
-        {
-                dock(control_signal, 0, 0, 0, 0, 0, 0);
-        } else if (index == 2)
-        {
-                dock(0, -control_signal, 0, 0, 0, 0, 0);
-        } else if (index == 3)
-        {
-                dock(0, 0, control_signal, 0, 0, 0, 0);
-        }else if (index == 4)
-        {
-                dock(0, 0, 0, control_signal, 0, 0, 0);
-        } else if (index == 5)
-        {
-                dock(0, 0, 0, 0, control_signal, 0, 0);
-        } else if (index == 6)
-        {
-                dock(0, 0, 0, 0, 0, control_signal, 0);
-        } /*else if (index == 7)
-        {
-                dock(0, 0, 0, 0, 0, 0, control_signal);
-        }*/
+        // Integrated error
+        int_errorY +=  curr_errorY * dt;
+        /*
+        // -- windup gaurd -- 
+        if (int_error < )
+        {}
+        else if ()
+        {}*/
+        
+        // differentiation
+        diffY = ((curr_errorY - prev_errorY) / dt);
+        
+        // scalling
+        p_termY = prop_gain  * curr_errorY;
+        i_termY = integ_gain * int_errorY;
+        d_termY = deriv_gain * diffY;
+        
+        // control signal
+        control_signalY = p_termY + i_termY + d_termY;
+        
+        
+        // save the current error as the previous one
+        // for the next iteration.
+        prev_errorY = curr_errorY;  
+        
+        // -------------------YAW--------------------------//
+        // e(t) = setpoint - actual value;
+        curr_errorYAW = RefYAW - MarPoseYAW;
+        
+        // Integrated error
+        int_errorYAW +=  curr_errorYAW * dt;
+        /*
+        // -- windup gaurd -- 
+        if (int_error < )
+        {}
+        else if ()
+        {}*/
+        
+        // differentiation
+        diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
+        
+        // scalling
+        p_termYAW = prop_gain  * curr_errorYAW;
+        i_termYAW = integ_gain * int_errorYAW;
+        d_termYAW = deriv_gain * diffYAW;
+        
+        // control signal
+        control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
+        
+        
+        // save the current error as the previous one
+        // for the next iteration.
+        prev_errorYAW = curr_errorYAW;  
+        
+        dock(-control_signalX, -control_signalY, -control_signalYAW);
 }
-
-//void ImageConverter::dock(double speed[6])
-void ImageConverter::dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW)
+void ImageConverter::dock(double VelX, double VelY, double omegaZ)
 {
         geometry_msgs::Twist msg;
         
-	/*msg.linear.x = speed[0];
-	msg.linear.y = speed[1];
-	msg.linear.z = speed[2];
-	
-	msg.angular.x = speed[3];
-	msg.angular.y = speed[4];
-	msg.angular.z = speed[5];*/
-	
 	msg.linear.x = VelX;
 	msg.linear.y = VelY;
-	msg.linear.z = VelZ;
-	
-	msg.angular.x = omegaX;
-	msg.angular.y = omegaY;
 	msg.angular.z = omegaZ;
 	
 	commandPub.publish(msg);
diff --git a/MobileRobot/AugReaMarker/CamMark/devel/lib/camtomar/camtomar b/MobileRobot/AugReaMarker/CamMark/devel/lib/camtomar/camtomar
index 37cb90a332aeda3501ade35e4881744c6442c1c8..c97d3b798cf20e95bfa133fb2fca7ea6b646bf8d 100755
Binary files a/MobileRobot/AugReaMarker/CamMark/devel/lib/camtomar/camtomar and b/MobileRobot/AugReaMarker/CamMark/devel/lib/camtomar/camtomar differ
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp
index 4c90b19eb7a2005a6eafb391f4bd966b86989a68..7b5dbffc159d2f18839fad624b74375d749232ac 100644
--- a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp
+++ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp
@@ -254,10 +254,10 @@ int main(int argc,char **argv){
 			}
 
 			// Print other rectangles that contains no valid markers
-			/** for (unsigned int i=0;i<MDetector.getCandidates().size();i++) {
+			 for (unsigned int i=0;i<MDetector.getCandidates().size();i++) {
 				aruco::Marker m( MDetector.getCandidates()[i],999);
 				m.draw(TheInputImageCopy,cv::Scalar(255,0,0));
-			}*/
+			}
 			// Draw a 3d cube in each marker if there is 3d info
 
                        if (TheCameraParameters.isValid()){
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~
index a168d8c91060306f44e96f8d3cfb7398fdce1393..4c90b19eb7a2005a6eafb391f4bd966b86989a68 100644
--- a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~
+++ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~
@@ -125,8 +125,15 @@ int main(int argc,char **argv){
 	if (readArguments(argc,argv)==false) {
 		return 0;
 	}
+        
+        // IP address for raw3_lund    
+        const std::string vsa = "http://192.168.0.101:8080/video?x.mjpeg";
 
-	TheVideoCapturer.open(1);
+        // -- publishing video stream with Android Camera--
+        TheVideoCapturer.open(vsa);
+
+        
+	//TheVideoCapturer.open(1);
 	// Check video is open
 	if (!TheVideoCapturer.isOpened()) {
 		cerr<<"Could not open video"<<endl;
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o b/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o
index a98ea80625f6ce742851db55d2e8a58eca16cc32..5432255b2c7a9608c246b223cdd85279282cc6ec 100644
Binary files a/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o and b/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o differ
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det b/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det
index 372c999c17a1eb60758e24d3413d35a9b9bd1948..f16c23cca75131daac0504890eb0355073b866f6 100755
Binary files a/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det and b/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det differ
diff --git a/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o b/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o
index 00a461ea1e92ab72127c7cafbb9f2fd7e06b9dde..d69e4b28ed6bff6038ab856d3bc7c55c705c7b1c 100644
Binary files a/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o and b/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o differ
diff --git a/MobileRobot/Controller/PID/devel/lib/dock/dock b/MobileRobot/Controller/PID/devel/lib/dock/dock
index 19e590c92ab00f26d0a46a9f264bfb3e84adf0e1..1acc90e1eeb0f363c1deb79f549dc8ac82561d4f 100755
Binary files a/MobileRobot/Controller/PID/devel/lib/dock/dock and b/MobileRobot/Controller/PID/devel/lib/dock/dock differ
diff --git a/MobileRobot/Controller/PID/dock/src/dock.cpp b/MobileRobot/Controller/PID/dock/src/dock.cpp
index 5743eb43e77a6168a17121cdba1a38610c562f20..cd75ca610f6bfa95bbe26c8db02b2792496c34b2 100644
--- a/MobileRobot/Controller/PID/dock/src/dock.cpp
+++ b/MobileRobot/Controller/PID/dock/src/dock.cpp
@@ -35,7 +35,6 @@ RobotDocking::RobotDocking() : ac("Robot_move", true)
         
         //PosSub = node.subscribe("/visualize_pose_ekf",1,&RobotDocking::RobCurrPose,this);
         
-
 }
 
 
diff --git a/MobileRobot/Controller/PID/dock/src/dock.cpp~ b/MobileRobot/Controller/PID/dock/src/dock.cpp~
index 2b5513180acd64439ee7f1975d84180942d16948..5743eb43e77a6168a17121cdba1a38610c562f20 100644
--- a/MobileRobot/Controller/PID/dock/src/dock.cpp~
+++ b/MobileRobot/Controller/PID/dock/src/dock.cpp~
@@ -45,7 +45,7 @@ void RobotDocking::ProgStart()
         
         while (ros::ok() && keepMoving)
         {
-                //dock(FORWARD_SPEED_MPS);
+                dock(FORWARD_SPEED_MPS);
                 ros::spinOnce();
                 rate.sleep();
         }