Difference between revisions of "Move With PID"

Revision as of 16:02, 26 August 2012

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;

Addition 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);
  }

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