PID Move

General

References

• ros_control
• control_toolbox
• Tutorial CMU PID

Indigo

Install PID

 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
    // TBD - pid.updatePid depreciated
    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

Build

Modifications to CMakeLists.txt v1

Add this to find_package

 control_toolbox

Add these to the Build section

 # move_it_pid
 add_executable(move_it_pid src/move_it_pid.cpp)
 target_link_libraries(move_it_pid
   ${catkin_LIBRARIES}
 )

In /home/eepp/catkin_ws.

 catkin_make

Execute

See Rotate_Using_CamShift Run. Substitute move_it with move_it_pid.

Investigate

PID Values

Using rostopic:

eepp@tabor:~$ rostopic echo /pid_values
x: 179.0
y: 268.794308389
z: 2.68794308389
---
x: 177.5
y: 266.532931749
z: 2.66532931749
---
x: 178.5
y: 268.061374099
z: 2.68061374099

Plot PID

TBD Update to Indigo. See Move With 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