PD controller for y and P controller for theta

MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp

@@ -92,14 +92,14 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
 //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 PID4Docking::RefPose[4] = {-.0957, -0.029968 /* Y_ref*/ ,  0.219607 /* X_ref*/ , -0.643852 /* theta_ref*/}; 
+const double PID4Docking::RefPose[4] = {-.0957, -0.0310121 /* Y_ref*/ ,  0.219607 /* X_ref*/ , -0.618508 /* theta_ref*/}; 
 
 // ----------------  PID gains---------------- //
-double PID4Docking::Kp_y = .55; //.55
+double PID4Docking::Kp_y = .49; //.55
 double PID4Docking::Ki_y = 0 ;//.002
-double PID4Docking::Kd_y = 0.6; //.1
+double PID4Docking::Kd_y = 0; //.1
 
-double PID4Docking::Kp_theta = .2;// .34 * Kp_y;  .//35 * Kp_y (u can't put that gain less than certain value since the left joint on the robot would hit the docking platform!)
+double PID4Docking::Kp_theta = .07;// .34 * Kp_y;  .//35 * Kp_y (u can't put that gain less than certain value since the left joint on the robot would hit the docking platform!)
 double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
 double PID4Docking::Kd_theta = 0; //* Kd_y; // .0008
 // ----------------  PID gains---------------- //
@@ -120,8 +120,8 @@ double PID4Docking::speed_reducer_theta = 1;
 
 // ------ offsets X, Y, theta for Docking ---------
 double PID4Docking::X_dock_thresh = .001;
-double PID4Docking::y_dock_thresh = .0013;
-double PID4Docking::theta_dock_thresh = (CV_PI/180) * 3; // 1 deg.
+double PID4Docking::y_dock_thresh = .0025;
+double PID4Docking::theta_dock_thresh = (CV_PI/180) * 1; // 1 deg.
 
 double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
 
@@ -449,8 +449,8 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
 	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");
-        ROS_INFO_STREAM(" Y_mar = " << camPose[1] << " vs Y_ref = " << RefPose[1] << " \n");
+	ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs. X_ref = " << RefPose[2] << " \n");
+        ROS_INFO_STREAM(" Y_mar = " << camPose[1] << " vs. Y_ref = " << RefPose[1] << " \n");
         
         ROS_INFO_STREAM(" theta_mar = " << camPose[3] << " rad. =~ " << (180/CV_PI) * camPose[3] << " deg. \n");
         ROS_INFO_STREAM(" theta_ref = " << RefPose[3] << " rad. =~ " << (180/CV_PI) * RefPose[3] << " deg. \n");

MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~

@@ -88,20 +88,18 @@ double PID4Docking::d_termYAW = 0;
 double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::control_signalYAW;
 // ---- CONTROLL PARAMETERS ------ //
 
-
 // ---- Ref. Values for Android Camera ---- //
 //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 PID4Docking::RefPose[4] = {-.0957, -0.029968 /* Y_ref*/ ,  0.219607 /* X_ref*/ , -0.643852 /* theta_ref*/}; 
 
 // ----------------  PID gains---------------- //
-double PID4Docking::Kp_y = .34; //.55
+double PID4Docking::Kp_y = .55; //.55
 double PID4Docking::Ki_y = 0 ;//.002
-double PID4Docking::Kd_y = 0; //.1
+double PID4Docking::Kd_y = 0.6; //.1
 
-double PID4Docking::Kp_theta = .19;// .34 * Kp_y;  .//35 * Kp_y (u can't put that gain less than certain value since the left joint on the robot would hit the docking platform!)
+double PID4Docking::Kp_theta = .2;// .34 * Kp_y;  .//35 * Kp_y (u can't put that gain less than certain value since the left joint on the robot would hit the docking platform!)
 double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
 double PID4Docking::Kd_theta = 0; //* Kd_y; // .0008
 // ----------------  PID gains---------------- //
@@ -121,9 +119,9 @@ double PID4Docking::speed_reducer_Y = 1;
 double PID4Docking::speed_reducer_theta = 1;
 
 // ------ offsets X, Y, theta for Docking ---------
-double PID4Docking::Pz_eps = .001;
-double PID4Docking::Py_eps = .002;
-double PID4Docking::A_eps = (CV_PI/180) * 3; // 1 deg.
+double PID4Docking::X_dock_thresh = .001;
+double PID4Docking::y_dock_thresh = .0013;
+double PID4Docking::theta_dock_thresh = (CV_PI/180) * 3; // 1 deg.
 
 double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
 
@@ -462,9 +460,9 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
 	{
 		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
-            		//(abs(RefPose[3] - camPose[3]) <= A_eps) // Yaw
+            		(abs(RefPose[2] - camPose[2]) <= X_dock_thresh) //&& // Z
+            		//(abs(RefPose[1] - camPose[1]) <= y_dock_thresh) && // Y
+            		//(abs(RefPose[3] - camPose[3]) <= theta_dock_thresh) // Yaw
             	)
         	{                        			
 			dock_finished = ros::Time::now().toSec();
@@ -482,8 +480,8 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
         	} else if (abs(RefPose[2] - camPose[2]) <= safety_margin_X)
 		{
 			if(
-				(abs(RefPose[1] - camPose[1]) > Py_eps) || 
-				(abs(abs(RefPose[3]) - abs(camPose[3])) > A_eps)
+				(abs(RefPose[1] - camPose[1]) > y_dock_thresh) || 
+				(abs(abs(RefPose[3]) - abs(camPose[3])) > theta_dock_thresh)
 			)
 			{	
 				ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m., Fixing Y or theta. \n ");     
@@ -491,8 +489,8 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
 				speed_reducer_theta = 1;		
 				Controller(RefPose[2], RefPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
 			} else if(
-				(abs(RefPose[1] - camPose[1]) <= Py_eps) && 
-				(abs(abs(RefPose[3]) - abs(camPose[3])) <= A_eps)				
+				(abs(RefPose[1] - camPose[1]) <= y_dock_thresh) && 
+				(abs(abs(RefPose[3]) - abs(camPose[3])) <= theta_dock_thresh)				
 				)
 			{
 				ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
@@ -516,7 +514,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 	ROS_INFO_STREAM("--------------------- Controller started ----------------------\n "); 
         
 	// -----------------X--------------------- //        
-	if(abs(RefX - MarPoseX) > Pz_eps)
+	if(abs(RefX - MarPoseX) > X_dock_thresh)
 	{			
 		/*// e(t) = setpoint - actual value;
         	curr_errorX = RefX - MarPoseX;
@@ -539,7 +537,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 	}
         // -----------------Y--------------------- // 
 	 
-	if((RefY - MarPoseY) < -Py_eps || (RefY - MarPoseY) > Py_eps)
+	if((RefY - MarPoseY) < -y_dock_thresh || (RefY - MarPoseY) > y_dock_thresh)
 	{	
 		// e(t) = setpoint - actual value;
         	curr_errorY = RefY - MarPoseY;
@@ -570,13 +568,13 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 		
         	prev_errorY = curr_errorY;
 	
-	} else if ((RefY - MarPoseY) <= Py_eps && (RefY - MarPoseY) >= -Py_eps)
+	} else if ((RefY - MarPoseY) <= y_dock_thresh && (RefY - MarPoseY) >= -y_dock_thresh)
 	{
 		control_signalY = 0;	
 	}
         
 	// -------------------YAW--------------------------//
-       if(abs(abs(RefYAW) - abs(MarPoseYAW)) > A_eps)
+       if(abs(abs(RefYAW) - abs(MarPoseYAW)) > theta_dock_thresh)
 	{	
 		// e(t) = setpoint - actual value;
 		curr_errorYAW = abs(RefYAW) - abs(MarPoseYAW);