PID has modified

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

@@ -92,18 +92,19 @@ 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.0310121 /* Y_ref*/ ,  0.219607 /* X_ref*/ , -0.618508 /* theta_ref*/}; 
+const double PID4Docking::RefPose[4] = {-.0957, 0.00903123 /* Y_ref*/ , 0.199791 /* X_ref*/ , -0.56 /* theta_ref*/}; 
 
 // ----------------  PID gains---------------- //
-double PID4Docking::Kp_y = .49; //.55
-double PID4Docking::Ki_y = 0 ;//.002
-double PID4Docking::Kd_y = 0; //.1
+double PID4Docking::Kp_y = .44; //.55
+double PID4Docking::Ki_y = .0005 ;//.002
+double PID4Docking::Kd_y = .15; //.1
 
-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::Kp_theta = .08;// .11
 double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
 double PID4Docking::Kd_theta = 0; //* Kd_y; // .0008
 // ----------------  PID gains---------------- //
 
+
 double PID4Docking::TT_S,PID4Docking::TT_E;
 // random pose initialized
 const double PID4Docking::y_up = .3; 
@@ -119,8 +120,8 @@ double PID4Docking::speed_reducer_Y = 1;
 double PID4Docking::speed_reducer_theta = 1;
 
 // ------ offsets X, Y, theta for Docking ---------
-double PID4Docking::X_dock_thresh = .001;
-double PID4Docking::y_dock_thresh = .0025;
+double PID4Docking::x_dock_thresh = .001;
+double PID4Docking::y_dock_thresh = .002; //.0015
 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
@@ -143,9 +144,9 @@ PID4Docking::PID4Docking()
 
     	// Publish pose message and buffer up to 100 messages
     	MarPose_pub = node_vis.advertise<geometry_msgs::PoseStamped>("/marker_pose", 100);
-    	commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",1);
+    	commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",100);
     
-    	MarPose_Sub = node_vis.subscribe("/marker_pose",1,&PID4Docking::camCB,this);
+    	MarPose_Sub = node_vis.subscribe("/marker_pose",100,&PID4Docking::camCB,this);
 }
 
   PID4Docking::~PID4Docking()
@@ -316,7 +317,8 @@ if (TheVideoCapturer.retrieve(TheInputImage))
 			
 		if (node_vis.ok() && found)
 		{
-		        y_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
+		        /*y_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0]; // changed !!! */ 
+			y_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
 			x_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
 			z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2];
 		
@@ -446,7 +448,8 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
         
 	
 	ROS_INFO_STREAM("--------- PID gains in trial no. " << docking_counter << " : ---------\n");
-	ROS_INFO_STREAM(" Kp = " << Kp_y << " ,  Ki = " << Ki_y << " , Kd = " << Kd_y << "\n");
+	ROS_INFO_STREAM(" Kp_y = " << Kp_y << " ,  Ki_y = " << Ki_y << " , Kd_y = " << Kd_y << "\n");        
+	ROS_INFO_STREAM(" Kp_theta = " << Kp_theta << " ,  Ki_theta = " << Ki_theta << " , Kd_theta = " << Kd_theta << "\n");
 
 	ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
 	ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs. X_ref = " << RefPose[2] << " \n");
@@ -460,7 +463,7 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
 	{
 		ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM ---------  \n ");
 		if (
-            		(abs(RefPose[2] - camPose[2]) <= X_dock_thresh) //&& // Z
+            		(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
             	)
@@ -505,7 +508,7 @@ camPose[3] = CamFB->pose.orientation.x; //  theta orientation
 	} else
 	{
   		ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ---------\n");		
-		RandomPose(x_new,y_new,theta_new);
+		Undocking(x_new,y_new,theta_new);
 	}
 }
 
@@ -514,7 +517,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 	ROS_INFO_STREAM("--------------------- Controller started ----------------------\n "); 
         
 	// -----------------X--------------------- //        
-	if(abs(RefX - MarPoseX) > X_dock_thresh)
+	if(abs(RefX - MarPoseX) > x_dock_thresh)
 	{			
 		/*// e(t) = setpoint - actual value;
         	curr_errorX = RefX - MarPoseX;
@@ -533,7 +536,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 		control_signalX = speed_reducer_X * 0.1;        
         } else
 	{
-		control_signalX = 5e-5;	
+		control_signalX = 0;	// 5e-5
 	}
         // -----------------Y--------------------- // 
 	 
@@ -560,11 +563,22 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
         	d_termY = Kd_y * diffY;
 
 		ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n");	      
-        	// control signal
+        	
         	control_signalY = p_termY + i_termY + d_termY;
         	
-		// robot & marker coordinates conversion
+		
+		//control_signalY = speed_reducer_Y * control_signalY;
+
+		/* -- MARKER IN CAMERA COORDINATATE FRAME */
+		if(MarPoseY < 0)
+		{
 			control_signalY = - speed_reducer_Y * control_signalY;
+			ROS_INFO("marker pose < 0 => robot is going to the RIGHT \n");
+		}else
+		{
+			control_signalY = speed_reducer_Y * control_signalY;
+			ROS_INFO("marker pose > 0 => robot is going to the LEFT \n");
+		}
 
         	prev_errorY = curr_errorY;
 	
@@ -590,7 +604,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
        		i_termYAW = Ki_theta * int_errorYAW;
        		d_termYAW = Kd_theta * diffYAW;
         	
-		ROS_INFO_STREAM("p_theta = " << p_termYAW << ", i_theta = " << i_termYAW << " d_theta = " << d_termYAW << " \n");	      
+		//ROS_INFO_STREAM("p_theta = " << p_termYAW << ", i_theta = " << i_termYAW << " d_theta = " << d_termYAW << " \n");	      
 		// control signal
         	control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
         	
@@ -606,13 +620,20 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 	
 		} else if (MarPoseYAW < 0 && abs(RefYAW) >= abs(MarPoseYAW))
 		{
-			ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation < 0 => CW rotation  \n"); // correct
-			control_signalYAW = - speed_reducer_theta * control_signalYAW;
+			/*ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation < 0 => CW rotation  \n"); // correct (changed)
+			control_signalYAW = - speed_reducer_theta * control_signalYAW;*/
+
+			ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation < 0 => CCW rotation  \n");
+			control_signalYAW = speed_reducer_theta * control_signalYAW;
 		
 		} else if (MarPoseYAW < 0 && abs(RefYAW) < abs(MarPoseYAW))
 		{
-			ROS_INFO("abs(RefYAW) < abs(orientation) && orientation < 0 => CCW rotation \n"); // correct ?
-			control_signalYAW = - speed_reducer_theta * control_signalYAW;
+			/*ROS_INFO("abs(RefYAW) < abs(orientation) && orientation < 0 => CCW rotation \n"); // correct ? (changed)
+			control_signalYAW = - speed_reducer_theta * control_signalYAW;*/
+
+			ROS_INFO("abs(RefYAW) < abs(orientation) && orientation < 0 => CW rotation \n");
+			control_signalYAW =  speed_reducer_theta * control_signalYAW;
+			
 		} else 
 		{
 			ROS_INFO("New Condition should be added! \n");
@@ -627,11 +648,13 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
 		control_signalYAW = 0;	
 	}
 
-        //ROS_INFO_STREAM("Control signalX = " << control_signalX <<" . \n");
-	//ROS_INFO_STREAM("Control signalY = " << control_signalY << ". \n");
-	//ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<". \n");
-	ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");	
+        /* ---   
+	ROS_INFO_STREAM("Control signalX = " << control_signalX <<"\n");
+	ROS_INFO_STREAM("Control signalY = " << control_signalY << "\n");
+	ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<"\n");
+	*/
 
+	ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");	
 	dock(control_signalX, control_signalY, control_signalYAW);
 }
 
@@ -696,7 +719,7 @@ void PID4Docking::GenerateRandomVal()
 	theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn; // will be used for Q_Learning
 }
 
-void PID4Docking::RandomPose(double X_rand, double Y_rand, double theta_rand)
+void PID4Docking::Undocking(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");

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/recordANDplaydata.txt~

@@ -43,3 +43,6 @@ z: ----
 
 $ rostopic echo -b NAME.bag -p /base_controller/odometry/pose/pose/position > POSITION.txt
 
+$ rostopic echo -b dock.bag -p /marker_pose/pose > Pose.txt
+
+$ rostopic echo -b dock.bag -p /base_controller/command > Vel.txt