From 963347dee125d46e5569c1e8fd45fd4032bf6702 Mon Sep 17 00:00:00 2001 From: Farid Alijani <farid.alijani@student.lut.fi> Date: Thu, 26 May 2016 21:23:09 +0200 Subject: [PATCH] PD controller for y and P controller for theta --- .../CamMark/camtomar/src/VisionControl.cpp | 16 ++++----- .../CamMark/camtomar/src/VisionControl.cpp~ | 36 +++++++++---------- 2 files changed, 25 insertions(+), 27 deletions(-) diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp index 0ac2d590..27c0b9f9 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp +++ b/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"); diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ index c7cb65de..0ac2d590 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ +++ b/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); -- GitLab