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

sepate P controller with threshold

parent 575ca4eb
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+ 72
− 52
View file @ 592ea405
......@@ -42,8 +42,6 @@
#include<VisionControl.h>
using namespace cv;
using namespace aruco;
using namespace std;
......@@ -96,30 +94,26 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
// ---- Ref. Values for Logitech Camera ---- //
const double PID4Docking::RefPose[4] = {-.0957, -0.030321 /* Y_ref*/ , 0.219427 /* X_ref*/ , -0.635269 /* theta_ref*/};
const double PID4Docking::RefPose[4] = {-.0957, -0.0311278 /* Y_ref*/ , 0.219607 /* X_ref*/ , -0.616442 /* theta_ref*/};
// ---------------- PID gains---------------- //
double PID4Docking::Kp_y = .65; //.7
double PID4Docking::Ki_y = .001;//.001
double PID4Docking::Kd_y = .2; // .2
double PID4Docking::Kp_y = .48; //.55
double PID4Docking::Ki_y = 0 ;//.002
double PID4Docking::Kd_y = 0; //.1
double PID4Docking::Kp_theta = .15 * Kp_y; // .2 * KP_y
double PID4Docking::Ki_theta = .6 * Ki_y; // .07 * Ki_y
double PID4Docking::Kd_theta = .00005 * Kd_y; // .00005 * Kd_y
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---------------- //
double PID4Docking::TT_S,PID4Docking::TT_E;
// random pose initialized
const double PID4Docking::y_up = .3;
const double PID4Docking::y_dwn = -.1;
const double PID4Docking::theta_dwn = -.7 /*-RefPose[3]*/;
const double PID4Docking::theta_up = .7 /*RefPose[3]*/;
double PID4Docking::x_new,PID4Docking::y_new,PID4Docking::theta_new;
double PID4Docking::dock_started,PID4Docking::dock_finished,PID4Docking::docking_duration;
double PID4Docking::speed_reducer_X = 1;
......@@ -127,17 +121,18 @@ double PID4Docking::speed_reducer_Y = 1;
double PID4Docking::speed_reducer_theta = 1;
// ------ offsets X, Y, theta for Docking ---------
double PID4Docking::x_thresh_dock = .001;
double PID4Docking::y_thresh_dock = .002;
double PID4Docking::theta_thresh_dock = (CV_PI/180) * 1; // 1 deg.
double PID4Docking::Pz_eps = .001;
double PID4Docking::Py_eps = .002;
double PID4Docking::A_eps = (CV_PI/180) * 5; // 1 deg.
double PID4Docking::safety_margin_X = .135; // safety margin X axis in docking process : 13.5 cm
double PID4Docking::safety_margin_X = .13; // 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()
......@@ -219,6 +214,7 @@ void PID4Docking::imageCb(const sensor_msgs::ImageConstPtr& msg)
img = cv_ptr->image;
}
bool PID4Docking::readArguments ( int argc,char **argv )
{
if (argc<2) {
......@@ -317,6 +313,8 @@ if (TheVideoCapturer.retrieve(TheInputImage))
found = false;
keepMoving = false;
ROS_INFO_STREAM("Marker is lost, successful docking trials : " << (docking_counter - 1) << "\n");
//RandomPose(x_new,y_new,theta_new);
//move2docking(-control_signalX, -control_signalY, control_signalYAW);
}
if (node_vis.ok() && found)
......@@ -425,7 +423,9 @@ key=cv::waitKey(30);
{
update_images = !update_images;
}
ros::spinOnce();
TT_E = ros::Time::now().toSec();
ROS_INFO_STREAM(" visualization while loop duration = " << (TT_E - TT_S) <<" \n");
......@@ -463,9 +463,9 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
{
ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM --------- \n ");
if (
(abs(RefPose[2] - camPose[2]) <= x_thresh_dock) //&& // Z
//(abs(RefPose[1] - camPose[1]) <= y_thresh_dock) && // Y
//(abs(RefPose[3] - camPose[3]) <= theta_thresh_dock) // Yaw
(abs(RefPose[2] - camPose[2]) <= Pz_eps) //&& // Z
//(abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y
//(abs(RefPose[3] - camPose[3]) <= A_eps) // Yaw
)
{
dock_finished = ros::Time::now().toSec();
......@@ -482,15 +482,19 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
// to make sure that y & theta are within the threshold...
} else if (abs(RefPose[2] - camPose[2]) <= safety_margin_X)
{
ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m., Fixing Y or theta. \n ");
if((abs(RefPose[1] - camPose[1]) > y_thresh_dock) || (abs(abs(RefPose[3]) - abs(camPose[3])) > theta_thresh_dock))
if(
(abs(RefPose[1] - camPose[1]) > Py_eps) ||
(abs(abs(RefPose[3]) - abs(camPose[3])) > A_eps)
)
{
ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m., Fixing Y or theta. \n ");
speed_reducer_Y = 1;
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]) <= y_thresh_dock) && (abs(abs(RefPose[3]) - abs(camPose[3])) <= theta_thresh_dock))
} else if(
(abs(RefPose[1] - camPose[1]) <= Py_eps) &&
(abs(abs(RefPose[3]) - abs(camPose[3])) <= A_eps)
)
{
ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
speed_reducer_X = .08;
......@@ -498,12 +502,13 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
}
}else
{
speed_reducer_X = 1;
Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
}
} else
{
ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ---------\n");
Undocking(x_new,y_new,theta_new);
RandomPose(x_new,y_new,theta_new);
}
}
......@@ -512,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_thresh_dock)
if(abs(RefX - MarPoseX) > Pz_eps)
{
/*// e(t) = setpoint - actual value;
curr_errorX = RefX - MarPoseX;
......@@ -535,7 +540,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
}
// -----------------Y--------------------- //
if((RefY - MarPoseY) < -y_thresh_dock || (RefY - MarPoseY) > y_thresh_dock)
if((RefY - MarPoseY) < -Py_eps || (RefY - MarPoseY) > Py_eps)
{
// e(t) = setpoint - actual value;
curr_errorY = RefY - MarPoseY;
......@@ -566,17 +571,16 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
prev_errorY = curr_errorY;
}else if ((RefY - MarPoseY) <= y_thresh_dock && (RefY - MarPoseY) >= -y_thresh_dock)
} else if ((RefY - MarPoseY) <= Py_eps && (RefY - MarPoseY) >= -Py_eps)
{
control_signalY = 0;
}
// -------------------YAW--------------------------//
if(abs(abs(RefYAW) - abs(MarPoseYAW)) > theta_thresh_dock)
if(abs(abs(RefYAW) - abs(MarPoseYAW)) > A_eps)
{
// e(t) = setpoint - actual value;
curr_errorYAW = RefYAW - MarPoseYAW;
curr_errorYAW = abs(RefYAW) - abs(MarPoseYAW);
// Integrated error
int_errorYAW += curr_errorYAW * dt;
......@@ -592,16 +596,30 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
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;
//control_signalYAW = - control_signalYAW;
if(MarPoseYAW > 0)
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;
} 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;
} 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;
} else if (MarPoseYAW < 0 && abs(RefYAW) < abs(MarPoseYAW))
{
ROS_INFO("Marker current orientation > 0, CCW rotation. \n");
ROS_INFO("abs(RefYAW) < abs(orientation) && orientation < 0 => CCW rotation \n"); // correct ?
control_signalYAW = - speed_reducer_theta * control_signalYAW;
} else
{
ROS_INFO("Marker current orientation < 0, CW rotation. \n");
control_signalYAW = speed_reducer_theta * control_signalYAW;
ROS_INFO("New Condition should be added! \n");
keepMoving = false;
}
// save the current error as the previous one
......@@ -611,12 +629,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");
//dock(control_signalX, control_signalY, control_signalYAW);
dock(control_signalX, control_signalY, control_signalYAW);
}
void PID4Docking::dock(double VelX, double VelY, double omegaZ)
......@@ -630,7 +649,7 @@ void PID4Docking::dock(double VelX, double VelY, double omegaZ)
commandPub.publish(msg);
ROS_INFO_STREAM(" Robot speed : \n" << msg);
ROS_INFO_STREAM(" Current speed of robot: \n" << msg << ".\n");
}
void PID4Docking::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
......@@ -680,7 +699,7 @@ void PID4Docking::GenerateRandomVal()
theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn; // will be used for Q_Learning
}
void PID4Docking::Undocking(double X_rand, double Y_rand, double theta_rand)
void PID4Docking::RandomPose(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");
......@@ -783,3 +802,4 @@ geometry_msgs::Twist msg_new;
commandPub.publish(msg_new);
}
MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+ 39
− 30
View file @ 592ea405
......@@ -42,8 +42,6 @@
#include<VisionControl.h>
using namespace cv;
using namespace aruco;
using namespace std;
......@@ -96,33 +94,27 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
// ---- Ref. Values for Logitech Camera ---- //
const double PID4Docking::RefPose[4] = {-.0957, -0.0295876 /* Y_ref*/ , 0.218695 /* X_ref*/ , -0.637745 /* theta_ref*/};
const double PID4Docking::RefPose[4] = {-.0957, -0.0311278 /* Y_ref*/ , 0.219607 /* X_ref*/ , -0.616442 /* theta_ref*/};
// ---------------- PID gains---------------- //
double PID4Docking::Kp_y = .7; //.7
double PID4Docking::Ki_y = .001;//.005
double PID4Docking::Kd_y = .2;
double PID4Docking::Kp_y = .48; //.55
double PID4Docking::Ki_y = 0 ;//.002
double PID4Docking::Kd_y = 0; //.1
double PID4Docking::Kp_theta = .2 * Kp_y;
double PID4Docking::Ki_theta = .6 * Ki_y; // .07 * Ki_y
double PID4Docking::Kd_theta = .00005 * Kd_y;
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::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;
const double PID4Docking::y_dwn = -.1;
const double PID4Docking::theta_dwn = -.7 /*-RefPose[3]*/;
const double PID4Docking::theta_up = .7 /*RefPose[3]*/;
double PID4Docking::x_new,PID4Docking::y_new,PID4Docking::theta_new;
double PID4Docking::dock_started,PID4Docking::dock_finished,PID4Docking::docking_duration;
double PID4Docking::zeroMax = .0000000000000000001;
double PID4Docking::zeroMin = -.0000000000000000001;
double PID4Docking::speed_reducer_X = 1;
double PID4Docking::speed_reducer_Y = 1;
......@@ -131,9 +123,9 @@ 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) * 1; // 1 deg.
double PID4Docking::A_eps = (CV_PI/180) * 5; // 1 deg.
double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
double PID4Docking::safety_margin_X = .13; // safety margin X axis in docking process : 18 cm
// ------ offsets X, Y, theta for Undocking ---------
double PID4Docking::x_thresh_undock = .02;
......@@ -222,6 +214,7 @@ void PID4Docking::imageCb(const sensor_msgs::ImageConstPtr& msg)
img = cv_ptr->image;
}
bool PID4Docking::readArguments ( int argc,char **argv )
{
if (argc<2) {
......@@ -509,6 +502,7 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
}
}else
{
speed_reducer_X = 1;
Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
}
} else
......@@ -586,8 +580,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
if(abs(abs(RefYAW) - abs(MarPoseYAW)) > A_eps)
{
// e(t) = setpoint - actual value;
curr_errorYAW = RefYAW - MarPoseYAW;
curr_errorYAW = abs(RefYAW) - abs(MarPoseYAW);
// Integrated error
int_errorYAW += curr_errorYAW * dt;
......@@ -603,15 +596,30 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
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;
//control_signalYAW = - control_signalYAW;
if(MarPoseYAW > 0)
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;
} 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;
} else if (MarPoseYAW < 0 && abs(RefYAW) >= abs(MarPoseYAW))
{
ROS_INFO("Marker current orientation > 0, CCW rotation. \n");
ROS_INFO("abs(RefYAW) >= abs(orientation) && orientation < 0 => CW rotation \n"); // correct
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;
} else
{
ROS_INFO("Marker current orientation < 0, CW rotation. \n");
control_signalYAW = speed_reducer_theta * control_signalYAW;
ROS_INFO("New Condition should be added! \n");
keepMoving = false;
}
// save the current error as the previous one
......@@ -794,3 +802,4 @@ geometry_msgs::Twist msg_new;
commandPub.publish(msg_new);
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment