Difference between revisions of "Move With PID"
From wikidb
Line 1: | Line 1: | ||
+ | == Modifications == | ||
+ | The move_it program introduced in [[Create and Build Follow]] is modified to use PID. | ||
+ | |||
+ | |||
+ | == Source == | ||
+ | === Addition to main === | ||
plot = nh.advertise<geometry_msgs::Point> | plot = nh.advertise<geometry_msgs::Point> | ||
("/pid_values", 1000); | ("/pid_values", 1000); | ||
+ | === New trackCallback === | ||
<pre> | <pre> | ||
Line 36: | Line 43: | ||
} | } | ||
</pre> | </pre> | ||
+ | |||
[[Image:PidPlot.jpg]] | [[Image:PidPlot.jpg]] |
Revision as of 15:32, 26 August 2012
Modifications
The move_it program introduced in Create and Build Follow is modified to use PID.
Source
Addition to main
plot = nh.advertise<geometry_msgs::Point> ("/pid_values", 1000);
New trackCallback
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); }