PID Move

General

Links

• ros_control
• control_toolbox

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

Other

PID Installation