Difference between revisions of "Move With PID"
From wikidb
(→Modifications) |
|||
(18 intermediate revisions by the same user not shown) | |||
Line 1: | Line 1: | ||
+ | == References == | ||
+ | |||
+ | * [http://docs.ros.org/diamondback/api/control_toolbox/html/classcontrol__toolbox_1_1Pid.html PID Reference] | ||
+ | |||
== 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 control_toolbox:Pid] package. | |
== Source == | == Source == | ||
− | |||
− | + | === Globals === | |
+ | |||
+ | The following globals are added. | ||
+ | |||
+ | #include "control_toolbox/pid.h" | ||
+ | |||
+ | ros::Publisher pid_pub; | ||
+ | control_toolbox::Pid pid; | ||
+ | ros::Time last_time; | ||
+ | |||
+ | |||
+ | === Additions 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); | ("/pid_values", 1000); | ||
=== New trackCallback === | === New trackCallback === | ||
− | <pre> | + | <pre> |
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 37: | Line 60: | ||
// 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> | ||
+ | === Listing === | ||
+ | |||
+ | [[Move It PID listing]] <tt>move_it_pid.cpp</tt> | ||
+ | |||
+ | == PID Values == | ||
+ | |||
+ | Using rostopic: | ||
+ | |||
+ | <pre> | ||
+ | eepp@tabor:~$ rostopic echo /pid_values | ||
+ | x: 122.0 | ||
+ | y: 190.026094422 | ||
+ | z: 1.90026094422 | ||
+ | --- | ||
+ | x: 106.0 | ||
+ | y: 158.420460842 | ||
+ | z: 1.58420460842 | ||
+ | --- | ||
+ | x: 88.0 | ||
+ | y: 131.313835442 | ||
+ | z: 1.31313835442 | ||
+ | </pre> | ||
+ | |||
+ | == Plot PID == | ||
+ | |||
+ | This command: | ||
+ | |||
+ | rxplot /pid_values/x,/pid_values/y /pid_values/z | ||
+ | |||
+ | Results in the following plot | ||
[[Image:PidPlot.jpg]] | [[Image:PidPlot.jpg]] | ||
+ | |||
+ | == References == | ||
+ | |||
+ | * PID Package [http://http://mediabox.grasp.upenn.edu/roswiki/doc/electric/api/control_toolbox/html/classcontrol__toolbox_1_1Pid.html control_toolbox:Pid] | ||
+ | * Tutorial [http://www.engin.umich.edu/group/ctm/PID/PID.html CMU PID] | ||
+ | |||
+ | = Installation = | ||
+ | [[PID Installation]] | ||
+ | http://ros.org/wiki/control_toolbox | ||
+ | |||
+ | root@tabor:stacks# svn co https://code.ros.org/svn/wg-ros-pkg/stacks/pr2_controllers/branches/pr2_controllers-1.4/control_toolbox/ | ||
+ | |||
+ | http://mediabox.grasp.upenn.edu/roswiki/doc/electric/api/control_toolbox/html/pid_8cpp_source.html |
Latest revision as of 14:28, 4 July 2016
Contents
References
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 globals are added.
#include "control_toolbox/pid.h"
ros::Publisher pid_pub; control_toolbox::Pid pid; ros::Time last_time;
Additions 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); }
Listing
Move It PID listing move_it_pid.cpp
PID Values
Using rostopic:
eepp@tabor:~$ rostopic echo /pid_values x: 122.0 y: 190.026094422 z: 1.90026094422 --- x: 106.0 y: 158.420460842 z: 1.58420460842 --- x: 88.0 y: 131.313835442 z: 1.31313835442
Plot PID
This command:
rxplot /pid_values/x,/pid_values/y /pid_values/z
Results in the following plot
References
- PID Package control_toolbox:Pid
- Tutorial CMU PID
Installation
PID Installation http://ros.org/wiki/control_toolbox
root@tabor:stacks# svn co https://code.ros.org/svn/wg-ros-pkg/stacks/pr2_controllers/branches/pr2_controllers-1.4/control_toolbox/
http://mediabox.grasp.upenn.edu/roswiki/doc/electric/api/control_toolbox/html/pid_8cpp_source.html