Commit ca137411 authored by Farid Alijani's avatar Farid Alijani
Browse files

x_dot adjus

parent 9f59e435
This diff is collapsed.
This diff is collapsed.
......@@ -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.0188 /* Y_ref*/ , -0.20007 /* X_ref*/ , -0.05 /* theta_ref*/};
const double PID4Docking::RefPose[4] = {-.0957, -0.0188 /* Y_ref*/ , -0.2025 /* X_ref*/ , -0.05 /* theta_ref*/};
//const double PID4Docking::RefPose[4] = {-.0957, 0.0185 /* Y_ref*/ , 0.2025 /* X_ref*/ , -0.05 /* theta_ref*/};
// ---------------- PID gains---------------- //
double PID4Docking::Kp_y = .28; //.4
double PID4Docking::Kp_y = .25; //.4
double PID4Docking::Ki_y = 0 ;// 0
double PID4Docking::Kd_y = 0.006; //.1
double PID4Docking::Kd_y = 0; //.1
double PID4Docking::Kp_theta = .25;// .18
double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
......@@ -535,9 +535,9 @@ void PID4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // su
{
ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
/*speed_reducer_X = .06; // for x_dot = .1 =>.06*/
speed_reducer_X = .04; // for x_dot = .15 =>.04 ---- optimal docking time and Y-axis offset ----
/*speed_reducer_X = .0375; // for x_dot = .16 =>.0375*/
speed_reducer_X = .06; // for x_dot = .1 =>.06
//speed_reducer_X = .04; // for x_dot = .15 =>.04 ---- optimal docking time and Y-axis offset ----
//speed_reducer_X = .0375; // for x_dot = .16 =>.0375
Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.1);
}
......@@ -575,8 +575,8 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
control_signalX = p_termX + i_termX + d_termX;
prev_errorX = curr_errorX;*/
//control_signalX = speed_reducer_X * 0.1;
control_signalX = speed_reducer_X * 0.15;
control_signalX = speed_reducer_X * 0.1;
//control_signalX = speed_reducer_X * 0.15;
//control_signalX = speed_reducer_X * 0.16;
} else
{
......@@ -656,11 +656,11 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
keepMoving = false;
}
/* ---*/
/* ---
ROS_INFO_STREAM("Control signalX = " << control_signalX <<"\n");
ROS_INFO_STREAM("Control signalY = " << control_signalY << "\n");
ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<"\n");
// dock(control_signalX, control_signalY, control_signalYAW);
// */ dock(control_signalX, control_signalY, control_signalYAW);
}
void PID4Docking::dock(double VelX, double VelY, double omegaZ)
......
......@@ -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.0185 /* Y_ref*/ , -0.2025 /* X_ref*/ , -0.05 /* theta_ref*/};
const double PID4Docking::RefPose[4] = {-.0957, -0.0188 /* Y_ref*/ , -0.20007 /* X_ref*/ , -0.05 /* theta_ref*/};
//const double PID4Docking::RefPose[4] = {-.0957, 0.0185 /* Y_ref*/ , 0.2025 /* X_ref*/ , -0.05 /* theta_ref*/};
// ---------------- PID gains---------------- //
double PID4Docking::Kp_y = .266; //.4
double PID4Docking::Ki_y = 0.0003 ;// 0
double PID4Docking::Kd_y = 0.014; //.1
double PID4Docking::Kp_y = .28; //.4
double PID4Docking::Ki_y = 0 ;// 0
double PID4Docking::Kd_y = 0.006; //.1
double PID4Docking::Kp_theta = .25;// .18
double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
......@@ -656,11 +656,11 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
keepMoving = false;
}
/* ---
/* ---*/
ROS_INFO_STREAM("Control signalX = " << control_signalX <<"\n");
ROS_INFO_STREAM("Control signalY = " << control_signalY << "\n");
ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<"\n");
// */ dock(control_signalX, control_signalY, control_signalYAW);
// dock(control_signalX, control_signalY, control_signalYAW);
}
void PID4Docking::dock(double VelX, double VelY, double omegaZ)
......
This diff is collapsed.
This diff is collapsed.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment