Move It PID listing

/*
 *  move_it
 *  Turn toward a tracked object.
 *
 *  Copyright (C) 2012, Ed C. Epp
 *  http://zdome.net/wiki/index.php/ROS
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <ros/ros.h>
#include "control_toolbox/pid.h"
#include "sensor_msgs/Image.h"
#include "image_transport/image_transport.h"
#include "cv_bridge/CvBridge.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Twist.h"
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <math.h>

#define SCREEN_WIDTH 320
#define MID_X (SCREEN_WIDTH / 2)
#define MIN_POWER 1
#define MAX_POWER 5
#define TURN_POWER 3
#define SCALE 500

ros::Publisher mover;
ros::Publisher pid_pub;
control_toolbox::Pid pid;
ros::Time last_time;

// ---------- 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);

    // Debugging Output
    /*
    ROS_INFO("error: %f    effort: %f\n", track_msg->x - MID_X, effort);
    ROS_INFO("trackCallback x: %d  y: %d  radius: %d    result: %f",
	     (int)track_msg->x, (int)track_msg->y, (int)track_msg->z,
             move_msg.angular.z);
    */
  }


// -----------------------------------------------------------------------------
// ---------- main -------------------------------------------------------------
// -----------------------------------------------------------------------------
int
main (int argc, char **argv)
{
  // Initialize ROS Node
  ros::init (argc, argv, "moveNode");
  ros::NodeHandle nh;

  mover = nh.advertise<geometry_msgs::Twist>
              ("/cmd_vel", 1000);
  pid_pub  = nh.advertise<geometry_msgs::Point>
              ("/pid_values", 1000);
  ros::Subscriber sub = nh.subscribe("/tracker", 1000, 
                    trackCallback);

  //pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3);
  pid.initPid(1.5, 1.0, 2.0, 0.3, -0.3);
  last_time = ros::Time::now();

  // Run forever
  ros::spin ();

  return 0;
}