Difference between revisions of "Move With PID"
From wikidb
(New page: Image:PidPlot.jpg) |
|||
| Line 1: | Line 1: | ||
| + | |||
| + | void trackCallback (const geometry_msgs::Point::ConstPtr & track_msg) | ||
| + | { | ||
| + | ros::NodeHandle nh; | ||
| + | geometry_msgs::Twist move_msg; | ||
| + | geometry_msgs::Point plot_msg; | ||
| + | ros::Time time = ros::Time::now(); | ||
| + | double effort = 0; | ||
| + | |||
| + | // Set the turn - turn on the z axis | ||
| + | move_msg.linear.x = 0; | ||
| + | move_msg.linear.y = 0; | ||
| + | move_msg.linear.z = 0; | ||
| + | move_msg.angular.x = 0; | ||
| + | move_msg.angular.y = 0; | ||
| + | |||
| + | // PID update | ||
| + | effort = pid.updatePid(track_msg->x - MID_X, time - last_time); | ||
| + | move_msg.angular.z = -effort / SCALE * MAX_POWER; | ||
| + | if (move_msg.angular.z > MAX_POWER) move_msg.angular.z = MAX_POWER; | ||
| + | if (move_msg.angular.z < -MAX_POWER) move_msg.angular.z = -MAX_POWER; | ||
| + | |||
| + | // Execute turn | ||
| + | mover.publish(move_msg); | ||
| + | |||
| + | // debug output | ||
| + | plot_msg.x = track_msg->x - MID_X; | ||
| + | plot_msg.y = - effort; | ||
| + | plot_msg.z = move_msg.angular.z; | ||
| + | plot.publish(plot_msg); | ||
| + | |||
| + | // Debugging Output | ||
| + | /* | ||
| + | ROS_INFO("error: %f effort: %f\n", track_msg->x - MID_X, effort); | ||
| + | ROS_INFO("trackCallback x: %d y: %d radius: %d result: %f", | ||
| + | (int)track_msg->x, (int)track_msg->y, (int)track_msg->z, | ||
| + | move_msg.angular.z); | ||
| + | */ | ||
| + | } | ||
| + | |||
[[Image:PidPlot.jpg]] | [[Image:PidPlot.jpg]] | ||
Revision as of 15:19, 26 August 2012
void trackCallback (const geometry_msgs::Point::ConstPtr & track_msg)
{
ros::NodeHandle nh;
geometry_msgs::Twist move_msg;
geometry_msgs::Point plot_msg;
ros::Time time = ros::Time::now();
double effort = 0;
// Set the turn - turn on the z axis move_msg.linear.x = 0; move_msg.linear.y = 0; move_msg.linear.z = 0; move_msg.angular.x = 0; move_msg.angular.y = 0;
// PID update effort = pid.updatePid(track_msg->x - MID_X, time - last_time); move_msg.angular.z = -effort / SCALE * MAX_POWER; if (move_msg.angular.z > MAX_POWER) move_msg.angular.z = MAX_POWER; if (move_msg.angular.z < -MAX_POWER) move_msg.angular.z = -MAX_POWER; // Execute turn mover.publish(move_msg);
// debug output plot_msg.x = track_msg->x - MID_X; plot_msg.y = - effort; plot_msg.z = move_msg.angular.z; plot.publish(plot_msg);
// Debugging Output
/*
ROS_INFO("error: %f effort: %f\n", track_msg->x - MID_X, effort);
ROS_INFO("trackCallback x: %d y: %d radius: %d result: %f",
(int)track_msg->x, (int)track_msg->y, (int)track_msg->z,
move_msg.angular.z); */ }
