Difference between revisions of "PID Move"
From wikidb
(→Install) |
|||
Line 9: | Line 9: | ||
== Install == | == Install == | ||
sudo apt-get install ros-indigo-ros-controllers | sudo apt-get install ros-indigo-ros-controllers | ||
+ | |||
+ | == 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 === | ||
+ | |||
+ | <pre> | ||
+ | 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> | ||
+ | |||
+ | === 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]] | ||
+ | |||
+ | == 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] | ||
= Jade = | = Jade = |
Revision as of 12:02, 8 November 2015
Contents
General
Links
Indigo
Install
sudo apt-get install ros-indigo-ros-controllers
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
Jade
Doing PID under Jade is abandoned for now. Briefly experimented with the installation instructions. Decided that working through the issues would be a distraction.
cd catkin_ws/src/ wstool init wstool merge https://raw.github.com/ros-controls/ros_control/indigo-devel/ros_control.rosinstall wstool update cd .. rosdep install --from-paths . --ignore-src --rosdistro jade -y ERROR: the following packages/stacks could not have their rosdep keys resolved