Difference between revisions of "Move With PID"
From wikidb
(→Addition to main) |
(→Source) |
||
| Line 4: | Line 4: | ||
== Source == | == Source == | ||
| + | |||
| + | == Globals == | ||
| + | |||
| + | The following are globals added. | ||
| + | |||
| + | #include "control_toolbox/pid.h" | ||
| + | |||
=== Addition to main === | === Addition to main === | ||
| Line 13: | Line 20: | ||
The following was added to main to publish messages that rxplot can subscribe to. | The following was added to main to publish messages that rxplot can subscribe to. | ||
| − | + | pid_plot = nh.advertise<geometry_msgs::Point> | |
("/pid_values", 1000); | ("/pid_values", 1000); | ||
=== New trackCallback === | === New trackCallback === | ||
| − | |||
void trackCallback (const geometry_msgs::Point::ConstPtr & track_msg) | void trackCallback (const geometry_msgs::Point::ConstPtr & track_msg) | ||
{ | { | ||
ros::NodeHandle nh; | ros::NodeHandle nh; | ||
geometry_msgs::Twist move_msg; | geometry_msgs::Twist move_msg; | ||
| − | geometry_msgs::Point | + | geometry_msgs::Point pid_pub_msg; |
ros::Time time = ros::Time::now(); | ros::Time time = ros::Time::now(); | ||
double effort = 0; | double effort = 0; | ||
| Line 44: | Line 50: | ||
// debug output | // debug output | ||
| − | + | pid_pub_msg.x = track_msg->x - MID_X; | |
| − | + | pid_pub_msg.y = - effort; | |
| − | + | pid_pub_msg.z = move_msg.angular.z; | |
| − | + | pid_pub.publish(pid_pub_msg); | |
} | } | ||
</pre> | </pre> | ||
Revision as of 15:53, 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
Globals
The following are globals added.
#include "control_toolbox/pid.h"
Addition to main
The following was added to main to initialize PID.
pid.initPid(1.5, 1.0, 2.0, 0.3, -0.3); last_time = ros::Time::now();
The following was added to main to publish messages that rxplot can subscribe to.
pid_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 pid_pub_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 pid_pub_msg.x = track_msg->x - MID_X; pid_pub_msg.y = - effort; pid_pub_msg.z = move_msg.angular.z; pid_pub.publish(pid_pub_msg); }
</pre>
Plot PID
This command:
rxplot /pid_values/x,/pid_values/y /pid_values/z
Results in the following plot
