myPoint.cpp

#include "inspec_pid/myPoint.h"

MyPoint::MyPoint(double xPos, double yPos, double alpha, ros::Time t)
{
  x = xPos;
  y = yPos;
  angle = alpha;
  time = t;
}

MyPoint::~MyPoint()
{
}

double MyPoint::getAngle(MyPoint* target)
{
  return atan2((target->y - y),(target->x - x));
}

double MyPoint::getDistance(MyPoint* target)
{
  return sqrt((pow(target->y - y,2.0)) + (pow(target->x - x,2.0)));
}

double MyPoint::getDistancex(MyPoint* target)
{
  return sqrt(pow(target->x - x,2.0));
}

double MyPoint::getDistancey(MyPoint* target)
{
  return sqrt(pow(target->x - x,2.0));
}

myPoint.h

#ifndef __MYPOINT_H
#define __MYPOINT_H
#include <math.h> 
#include "ros/ros.h"

class MyPoint
{
public:

  /* Constructor:
   * xPos   Position in x.
   * yPos   Position in y.
   * alpha  Robot rotation.
   * t      Time of measurement
   */
  MyPoint(double xPos, double yPos, double alpha, ros::Time t);

  ~MyPoint();

  /* Returns angle between this and target point.
   */
  double getAngle(MyPoint* target);

  /* Returns distance between this and target point.
   */
  double getDistance(MyPoint* target);
  double getDistancex(MyPoint* target);
  double getDistancey(MyPoint* target);

//variables
  double x;       // Position in x.
  double y;       // Position in y.
  ros::Time time; // Time, when the position was measured.
  double angle;   // Robot's angle.
};
#endif

node_pid.cpp

#include "inspec_pid/node_pid.h"
#include <math.h>
#define PI 3.141592
#define F_KP 2.58   // P constant for PID translation controller
#define F_KD 0.047  // D constant for PID translation controller
#define F_KI 0.0   // I constant for PID translation controller
#define R_KP 2.0   // P constant for PID rotation controller
#define R_KD 0.1   // D constant for PID rotation controller
#define R_KI 0.0   // I constant for PID rotation controller

NodePID::NodePID(ros::Publisher pub, double tol, double tolA, double distx, double disty, double ang, double mSpeed, double mASpeed, int nbr)
{
  targetDistancex = distx;
  targetDistancey = disty;
 targetDistance=sqrt(pow(distx,2.0)+pow(disty,2.0));
  tolerance = tol;
  toleranceAngle = tolA;
  targetAngle = ang;
  maxSpeed = mSpeed;
  maxASpeed = mASpeed;  
  pubMessage = pub;
  iterations = 0;
  sumDistancex = 0;
  sumDistancey = 0;
  sumAngle = 0;
  start = new MyPoint(0.0, 0.0, 0.0, ros::Time::now());
  last  = new MyPoint(0.0, 0.0, 0.0, ros::Time::now());
  ROS_INFO_STREAM( " i =  " << nbr);
}

NodePID::~NodePID()
{
}

//Publisher
void NodePID::publishMessage(double angleCommand, double speedCommandX, double speedCommandY)
{
  //preparing message
  geometry_msgs::Twist msg;

  msg.linear.x = speedCommandX;
  msg.linear.y = speedCommandY;
  msg.angular.z = angleCommand;

  //publishing message
  pubMessage.publish(msg);
}

//Subscriber
void NodePID::messageCallback(const tum_ardrone::filter_state& msg)
{
  double angleCommand = 0;
  double speedCommandX = 0;
  double speedCommandY = 0;
  MyPoint* actual = new MyPoint(msg.x, msg.y, msg.yaw, msg.header.stamp);

  if (iterations == 0)
  {
    start->x = actual->x;
    start->y = actual->y;
    start->time = actual->time;
    start->angle = actual->angle;
    last->x = actual->x;
    last->y = actual->y;
    last->time = actual->time;
    last->angle = actual->angle;
  }
  iterations++;

  //Calculation of action intervention.
 if (closeEnough(actual) == true)
  {
    ROS_INFO("GOAL ACHIEVED");
    publishMessage(0.0,0.0,0.0);

 return;
  }
  
  else {
  if (fabs(actual->x )<= fabs(targetDistancex) )
  {
ROS_INFO_STREAM ("Acheive_y   " << start->getDistancex(actual));
    speedCommandX = calculatePID_x(actual,start->getDistancex(actual)*copysign(1.0, targetDistancex),start->getDistancex(last)*copysign(1.0, targetDistancex),targetDistancex,F_KP,F_KD,F_KI,&sumDistancex);
  publishMessage(fmin(maxASpeed,angleCommand), fmin(maxSpeed,speedCommandX), fmin(maxSpeed,speedCommandY));
}
    if (fabs(actual->y)<= fabs(targetDistancey) )
{   
 ROS_INFO_STREAM ("Acheive_x   " << start->getDistancey(actual));
speedCommandY = calculatePID_y(actual,start->getDistancey(actual)*copysign(1.0, targetDistancey),start->getDistancey(last)*copysign(1.0, targetDistancey),targetDistancey,F_KP,F_KD,F_KI,&sumDistancey);
  publishMessage(fmin(maxASpeed,angleCommand), fmin(maxSpeed,speedCommandX), fmin(maxSpeed,speedCommandY)); 
 }

  if (actual->angle-last->angle < -PI)
  {
    actual->angle += 2*PI;
  } 
  else if (actual->angle-last->angle > PI)
  {
    actual->angle -= 2*PI;
  }

  angleCommand = calculatePID_x(actual,actual->angle-start->angle, last->angle-start->angle,targetAngle,R_KP,R_KD,R_KI,&sumAngle);

  //Saving position to last
  last->x = actual->x;
  last->y = actual->y;
  last->time = actual->time;
  last->angle = actual->angle;

  //Invoking method for publishing message
ROS_INFO_STREAM( "     x  " << last->x << "    y  " << last->y) ;
ROS_INFO_STREAM( " targetDistancey  " <<targetDistancey << "  targetDistancex  " << targetDistancex << " targetAngle " << targetAngle );
}
}
bool NodePID::closeEnough(MyPoint* actual)
{
  double distance;
  distance = start->getDistance(actual)*copysign(1.0, targetDistance);
  if (fabs(distance-targetDistance) > tolerance)
  {
    return false;
  }
  if (fabs(targetAngle - (actual->angle - start->angle)) > toleranceAngle &
    fabs(targetAngle - (actual->angle - start->angle) + 2*PI) > toleranceAngle &
    fabs(targetAngle - (actual->angle - start->angle) - 2*PI) > toleranceAngle)
  {
    return false;
  }
  return true;
}

double NodePID::calculatePID_x(MyPoint* actual, double actualValue_x, double lastValue_x, double reference_x, double kP, double kD, double kI, double *sum_x)
{
  double speed_x = 0;
  double error_x = reference_x - actualValue_x;
  double previousError_x = reference_x - lastValue_x;
  double dt_x = actual->time.toSec() - last->time.toSec();
  double derivative_x = (error_x - previousError_x)/dt_x;
  *sum_x = *sum_x + error_x*dt_x;
  speed_x = kP*error_x + kD*derivative_x + kI*(*sum_x);
  return speed_x;
}

double NodePID::calculatePID_y(MyPoint* actual, double actualValue_y, double lastValue_y, double reference_y, double kP, double kD, double kI, double *sum_y)
{
  double speed_y = 0;
  double error_y = reference_y - actualValue_y;
  double previousError_y = reference_y - lastValue_y;
  double dt_y = actual->time.toSec() - last->time.toSec();
  double derivative_y = (error_y - previousError_y)/dt_y;
  *sum_y = *sum_y + error_y*dt_y;
  speed_y = kP*error_y + kD*derivative_y + kI*(*sum_y);
  return speed_y;
}

node_pid.h

#ifndef __NODEPID_H
#define __NODEPID_H
#include "myPoint.h"
#include "ros/ros.h"
#include "tum_ardrone/filter_state.h"
#include "geometry_msgs/Twist.h"
#include <queue>

class NodePID
{
public:

  /* Constructor:
   * 
   * pub     Publisher, which can send commands to robot.
   * tol     Position tolerance [link](#m).
   * tolA    Angle tolerance [link](#m).
   * dist    Robot will go forward this distance.
   * ang     Robot will turn by this angle [link](#rad).
   * mSpeed  Maximum speed of robot.
   * mASpeed Maximum angular speed of robot.
   */
  NodePID(ros::Publisher pub, double tol, double tolA, double disty,double distx,double ang, double mSpeed, double mASpeed, int nbr);

  ~NodePID();

  /* This method publishes commands for robot.
   *
   * angleCommand   Angular velocity.
   * speedCommand   Velocity.
   */
  void publishMessage(double angleCommand, double speedCommandX, double speedCommandY);

  /* This method reads data from sensor and processes them to variables.
   * It saves actual position, calls methods for evaluating 
   * speed and calls method for publishing message.
   * 
   * msg   Message, which contains odometry data.
   */
  void messageCallback(const tum_ardrone::filter_state& msg);

  /* This method calculates, if robot is close enough from target point.
   * If distance between robot and target point is shorter than 
   * tolerance, target is accomplished and method returns true.
   * 
   * actual   Actual position of robot.
   */
  bool closeEnough(MyPoint* actual);

  /* This method calculates action intervention from PSD controller.
   * 
   * actual      Actual position of robot.
   * actualValue Actual output value.
   * lastValue   Output value from one step ago.
   * reference   Reference value.
   * kP          P constant for controller PSD.
   * kD          D constant for controller PSD.
   * kS          S constant for controller PSD.
   * sum         sum of errors.
   */
  double calculatePID_x(MyPoint* actual, double actualValue_x, double lastValue_x, double reference_x, double kP, double kD, double kI, double *sum_x);
  double calculatePID_y(MyPoint* actual, double actualValue_y, double lastValue_y, double reference_y, double kP, double kD, double kI, double *sum_y);
//variables
  MyPoint *start;     // Start position. Distance will be measured from here
  MyPoint *last;      // Last position of robot.
  double tolerance;   // Tolerated deviation from target distance.
  double maxSpeed;    // Maximal velocity.
  double maxASpeed;   // Maximal angular velocity.
  ros::Publisher pubMessage; // Object for publishing messages.
  double targetDistancex; // Robot will go forwrd this distance
  double targetDistancey; // Robot will go forwrd this distance
  double targetDistance; // Robot will go forwrd this distance
  double targetAngle;    // Robot will turn by this angle.
  int iterations;        // Number of received messages.
  double sumDistancex;    // Sum of distance errors for PID controller.
  double sumDistancey;    // Sum of distance errors for PID controller.
  double sumAngle;       // Sum of angle errors for PID controller.
  double toleranceAngle; // Tolerated deviation from target angle.
  int nbr;               //just a number
};

#endif 

pid.cpp

#include "inspec_pid/myPoint.h"
#include "inspec_pid/node_pid.h"

#define SUBSCRIBER_BUFFER_SIZE 1  // Size of buffer for subscriber.
#define PUBLISHER_BUFFER_SIZE 1000 // Size of buffer for publisher.
#define TOLERANCE 0.1  // Distance from the target distance, at which the distance will be considered as achieved.
#define TOLERANCE_ANGLE 0.2  // Differenc from target angle, which will be tolerated.
#define MAX_SPEED 0.5    // Maximum speed of robot.
#define MAX_A_SPEED 2.0    // Maximum angular speed of robot.
// #define PUBLISHER_TOPIC "/syros/base_cmd_vel"
#define PUBLISHER_TOPIC "/cmd_vel"
// #define SUBSCRIBER_TOPIC "/ardrone/predictedPose"
#define SUBSCRIBER_TOPIC "/ardrone/predictedPose"
#define PI 3.141592


double tag_x = 0;
double tag_y = 0;
double angle = 0.0;
double ang;
int i;
ros::Publisher pub ;
ros::Subscriber sub;
NodePID *nodePID(0);
int main(int argc, char **argv)
{
  //Initialization of node
  ros::init(argc, argv, "pid_control");
 ros::NodeHandle n;
// Space between the spirals
ang=1;
tag_x=ang * cos(ang);
tag_y=ang * sin(ang);
  pub = n.advertise<geometry_msgs::Twist>(PUBLISHER_TOPIC, PUBLISHER_BUFFER_SIZE);

 nodePID= new NodePID(pub, TOLERANCE, TOLERANCE_ANGLE,tag_x,tag_y, 0 , MAX_SPEED, MAX_A_SPEED,1);
sub = n.subscribe(SUBSCRIBER_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodePID::messageCallback, nodePID);


for(i=1;i<200;i=+10){
ang=i;
tag_x=ang * cos(ang);
tag_y=ang * sin(ang);
ROS_INFO_STREAM( " i=  " << i );

  //Creating object, which stores data from sensors and has methods for
  //publishing and subscribing
  //Creating subscriber and publisher
   //Creating publisher

 nodePID= new NodePID(pub, TOLERANCE, TOLERANCE_ANGLE,tag_x,tag_y, angle, MAX_SPEED, MAX_A_SPEED,i);

 ros::Rate loop_rate(100);
 loop_rate.sleep();
 ros::spinOnce();
}

  return 0;
}