Commit 9f59e435 authored by Farid Alijani's avatar Farid Alijani
Browse files

x_dot adjust

parent 7c918604
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.0185 /* Y_ref*/ , -0.2025 /* 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 = .266; //.4
double PID4Docking::Ki_y = 0.0003 ;// 0
double PID4Docking::Kd_y = 0.014; //.1
double PID4Docking::Kp_y = .25; //.4
double PID4Docking::Ki_y = 0 ;// 0
double PID4Docking::Kd_y = 0; //.1
double PID4Docking::Kp_theta = .25;// .18
double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
......@@ -379,6 +379,21 @@ void PID4Docking::ProgStart(int argc,char** argv)
}
} else
{
// yaw has an offset of 180 deg.
if (yaw > 0)
{
theta_with_offset = yaw - y_off;
} else
{
theta_with_offset = yaw + y_off;
}
x_robINmar_coor = z_t * cos(yaw) - y_t * sin(yaw);
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);
/*//if ((yaw - (CV_PI - RefPose[3])) > RefPose[3])
if (yaw > 0)
{
......@@ -386,14 +401,15 @@ void PID4Docking::ProgStart(int argc,char** argv)
theta_with_offset = yaw_prev + y_off;
x_robINmar_coor = z_t * cos(yaw_prev) - y_t * sin(yaw_prev);
y_robINmar_coor = -z_t * sin(yaw_prev) + y_t * cos(yaw_prev);
} else
//} else if ((yaw - (CV_PI - RefPose[3])) < RefPose[3])
} else if (yaw < 0)
{
yaw_prev = yaw;
msg_t.pose.orientation.z = yaw;
theta_with_offset = yaw + y_off;
x_robINmar_coor = z_t * cos(yaw) - y_t * sin(yaw);
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);
}
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);
}*/
}
Transpose_pub.publish(msg_t);
......@@ -537,9 +553,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 = .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 = .0375; // for x_dot = .16 =>.0375
Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.1);
}
......@@ -579,7 +595,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
//control_signalX = speed_reducer_X * 0.1;
control_signalX = speed_reducer_X * 0.15;
//control_signalX = speed_reducer_X * 0.16;
//control_signalX = speed_reducer_X * 0.16;
} else
{
control_signalX = 0; // 5e-5
......@@ -658,11 +674,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.
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