Move With PID

References

• PID Reference

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