From 395f98eeee24946837f7d22925dfd158075b5254 Mon Sep 17 00:00:00 2001
From: Farid Alijani <farid.alijani@student.lut.fi>
Date: Wed, 25 May 2016 18:12:01 +0200
Subject: [PATCH] PD controller for y and P controller for theta
---
.../CamMark/camtomar/src/VisionControl.cpp | 39 +++++++++----------
.../CamMark/camtomar/src/VisionControl.cpp~ | 11 +++---
2 files changed, 23 insertions(+), 27 deletions(-)
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 49803d71..0ac2d590 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -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);
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 5a86b9a7..c7cb65de 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -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()
--
GitLab