Difference between revisions of "PID Move"

From wikidb
Jump to: navigation, search
(→‎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

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

PidPlot.jpg

References

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

Retrieved from "http://zdome.net/wiki/index.php?title=PID_Move&oldid=1893"