Difference between revisions of "Move With PID"
From wikidb
(→Modifications) |
|||
| Line 1: | Line 1: | ||
== Modifications == | == Modifications == | ||
The move_it program introduced in [[Create and Build Follow]] is modified to use PID. | The move_it program introduced in [[Create and Build Follow]] is modified to use PID. | ||
| − | See the [http://http://mediabox.grasp.upenn.edu/roswiki/doc/electric/api/control_toolbox/html/classcontrol__toolbox_1_1Pid.html | + | See the [http://http://mediabox.grasp.upenn.edu/roswiki/doc/electric/api/control_toolbox/html/classcontrol__toolbox_1_1Pid.html control_toolbox:Pid] package. |
== Source == | == Source == | ||
Revision as of 15:43, 26 August 2012
Modifications
The move_it program introduced in Create and Build Follow is modified to use PID. See the control_toolbox:Pid package.
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);
}
Plot PID
This command:
rxplot /pid_values/x,/pid_values/y /pid_values/z
Results in the following plot
