Skip to content
Snippets Groups Projects
Commit 395f98ee authored by Farid Alijani's avatar Farid Alijani
Browse files

PD controller for y and P controller for theta

parent 592ea405
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+ 18
− 21
View file @ 395f98ee
......@@ -88,18 +88,16 @@ 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.0311278 /* Y_ref*/ , 0.219607 /* X_ref*/ , -0.616442 /* theta_ref*/};
const double PID4Docking::RefPose[4] = {-.0957, -0.029968 /* Y_ref*/ , 0.219607 /* X_ref*/ , -0.643852 /* theta_ref*/};
// ---------------- PID gains---------------- //
double PID4Docking::Kp_y = .48; //.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 = .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
......@@ -121,18 +119,17 @@ 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) * 5; // 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 = .13; // safety margin X axis in docking process : 18 cm
double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
// ------ offsets X, Y, theta for Undocking ---------
double PID4Docking::x_thresh_undock = .02;
double PID4Docking::y_thresh_undock = .015;
double PID4Docking::theta_thresh_undock = (CV_PI/180) * 3;
double PID4Docking::docking_counter = 1;
PID4Docking::PID4Docking()
......@@ -463,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();
......@@ -483,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 ");
......@@ -492,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");
......@@ -517,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;
......@@ -540,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;
......@@ -571,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);
......
MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+ 5
− 6
View file @ 395f98ee
......@@ -94,14 +94,14 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
// ---- Ref. Values for Logitech Camera ---- //
const double PID4Docking::RefPose[4] = {-.0957, -0.0311278 /* Y_ref*/ , 0.219607 /* X_ref*/ , -0.616442 /* theta_ref*/};
const double PID4Docking::RefPose[4] = {-.0957, -0.029968 /* Y_ref*/ , 0.219607 /* X_ref*/ , -0.643852 /* theta_ref*/};
// ---------------- PID gains---------------- //
double PID4Docking::Kp_y = .48; //.55
double PID4Docking::Kp_y = .34; //.55
double PID4Docking::Ki_y = 0 ;//.002
double PID4Docking::Kd_y = 0; //.1
double PID4Docking::Kp_theta = .22;// .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 = .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::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
double PID4Docking::Kd_theta = 0; //* Kd_y; // .0008
// ---------------- PID gains---------------- //
......@@ -123,16 +123,15 @@ 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) * 5; // 1 deg.
double PID4Docking::A_eps = (CV_PI/180) * 3; // 1 deg.
double PID4Docking::safety_margin_X = .13; // safety margin X axis in docking process : 18 cm
double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
// ------ offsets X, Y, theta for Undocking ---------
double PID4Docking::x_thresh_undock = .02;
double PID4Docking::y_thresh_undock = .015;
double PID4Docking::theta_thresh_undock = (CV_PI/180) * 3;
double PID4Docking::docking_counter = 1;
PID4Docking::PID4Docking()
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment