From 30836fade7b90557ee768b23a5b9739a084925fe Mon Sep 17 00:00:00 2001
From: ajit2704 <kaleajit27@gmail.com>
Date: Sun, 5 Mar 2017 01:15:31 +0530
Subject: [PATCH 1/5] assignment1

---
 assignment_1/ajit2704_print_squares | 1 +
 1 file changed, 1 insertion(+)
 create mode 160000 assignment_1/ajit2704_print_squares

diff --git a/assignment_1/ajit2704_print_squares b/assignment_1/ajit2704_print_squares
new file mode 160000
index 0000000..668d65d
--- /dev/null
+++ b/assignment_1/ajit2704_print_squares
@@ -0,0 +1 @@
+Subproject commit 668d65ded2da52e87abcafc8c9b85e0317b0987d

From ad8b95341055dab8fb1c06c57bb737119e715abc Mon Sep 17 00:00:00 2001
From: ajit2704 <kaleajit27@gmail.com>
Date: Sun, 5 Mar 2017 12:13:42 +0530
Subject: [PATCH 2/5] assignment2

---
 .../ajit2704_pid_control/CMakeLists.txt       |  36 +++++
 assignment_2/ajit2704_pid_control/Makefile    |   1 +
 .../ajit2704_pid_control/include/header.h     |  23 ++++
 .../ajit2704_pid_control/include/node.h       |  45 +++++++
 .../ajit2704_pid_control/manifest.xml         |  14 ++
 .../ajit2704_pid_control/src/header.cpp       |  23 ++++
 .../ajit2704_pid_control/src/node.cpp         | 123 ++++++++++++++++++
 assignment_2/ajit2704_pid_control/src/pid.cpp |  34 +++++
 8 files changed, 299 insertions(+)
 create mode 100644 assignment_2/ajit2704_pid_control/CMakeLists.txt
 create mode 100644 assignment_2/ajit2704_pid_control/Makefile
 create mode 100644 assignment_2/ajit2704_pid_control/include/header.h
 create mode 100644 assignment_2/ajit2704_pid_control/include/node.h
 create mode 100644 assignment_2/ajit2704_pid_control/manifest.xml
 create mode 100644 assignment_2/ajit2704_pid_control/src/header.cpp
 create mode 100644 assignment_2/ajit2704_pid_control/src/node.cpp
 create mode 100644 assignment_2/ajit2704_pid_control/src/pid.cpp

diff --git a/assignment_2/ajit2704_pid_control/CMakeLists.txt b/assignment_2/ajit2704_pid_control/CMakeLists.txt
new file mode 100644
index 0000000..5ac565f
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/CMakeLists.txt
@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+set (SRCS1 ${SRCS1} src/Point.cpp) 
+set (SRCS1 ${SRCS1} src/node.cpp) 
+set (SRCS1 ${SRCS1} src/pid.cpp)
+include_directories(include ${catkin_INCLUDE_DIRS})
+add_executable(pid ${SRCS1}) 
+target_link_libraries(pid ${catkin_LIBRARIES}) 
diff --git a/assignment_2/ajit2704_pid_control/Makefile b/assignment_2/ajit2704_pid_control/Makefile
new file mode 100644
index 0000000..b75b928
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/Makefile
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/assignment_2/ajit2704_pid_control/include/header.h b/assignment_2/ajit2704_pid_control/include/header.h
new file mode 100644
index 0000000..1b1ccff
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/include/header.h
@@ -0,0 +1,23 @@
+#include <math.h> 
+#include "ros/ros.h"
+
+class Point
+{
+public:
+
+  
+  Point(double xpos, double ypos, double theta, ros::Time t);
+
+  ~Point();
+
+ 
+  double getAngle(Point* target);
+
+  
+  double getDistance(Point* target);
+
+  double x;      
+  double y;      
+  ros::Time time;
+  double angle;  
+};
diff --git a/assignment_2/ajit2704_pid_control/include/node.h b/assignment_2/ajit2704_pid_control/include/node.h
new file mode 100644
index 0000000..ce0b939
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/include/node.h
@@ -0,0 +1,45 @@
+#include "header.h"
+#include "ros/ros.h"
+#include "nav_msgs/Odometry.h"
+#include "geometry_msgs/Twist.h"
+#include <queue>
+
+class NodePID
+{
+public:
+
+  
+  NodePID(ros::Publisher pub, double tol, double tolA, double dist, double ang, double mSpeed, double mASpeed);
+
+  ~NodePID();
+
+  
+  void publishMessage(double angleCommand, double speedCommand);
+
+ /* This method reads data from sensor and processes them to variables.
+   */
+      
+  void messageCallback(const nav_msgs::Odometry::ConstPtr& msg);
+
+  /*if robot is close enough from target point, target is accomplished and method returns true.
+   */
+  bool closeEnough(Point* actual);
+
+  /* This method calculates action intervention from PSD controller.
+   */
+  double calculatePSD(Point* actual, double actualValue, double lastValue, double reference, double kP, double kD, double kS, double *sum);
+
+//variables
+  Point *start;     // Start position. Distance will be measured from here
+  Point *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 targetDistance; // Robot will go forward this distance.
+  double targetAngle;    // Robot will turn by this angle.
+  int iterations;        // Number of received messages.
+  double sumDistance;    // Sum of distance errors for PSD controller.
+  double sumAngle;       // Sum of angle errors for PSD controller.
+  double toleranceAngle; // Tolerated deviation from target angle.
+};
diff --git a/assignment_2/ajit2704_pid_control/manifest.xml b/assignment_2/ajit2704_pid_control/manifest.xml
new file mode 100644
index 0000000..cadf020
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/manifest.xml
@@ -0,0 +1,14 @@
+<package>
+  <description brief="ajit2704_pid_control">
+
+     ajit2704_pid_control
+
+  </description>
+  <author>ajit</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/ajit2704_pid_control</url>
+
+</package>
+
+
diff --git a/assignment_2/ajit2704_pid_control/src/header.cpp b/assignment_2/ajit2704_pid_control/src/header.cpp
new file mode 100644
index 0000000..70f50b9
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/src/header.cpp
@@ -0,0 +1,23 @@
+#include "header.h"
+
+Point::Point(double xpos, double ypos, double theta, ros::Time t)
+{
+  x = xpos;
+  y = ypos;
+  angle = theta;
+  time = t;
+}
+
+Point::~Point()
+{
+}
+
+double Point::getAngle(Point* target)
+{
+  return atan2((target->y - y),(target->x - x));
+}
+
+double Point::getDistance(Point* target)
+{
+  return sqrt((pow(target->y - y,2.0)) + (pow(target->x - x,2.0)));
+}
diff --git a/assignment_2/ajit2704_pid_control/src/node.cpp b/assignment_2/ajit2704_pid_control/src/node.cpp
new file mode 100644
index 0000000..64ec00b
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/src/node.cpp
@@ -0,0 +1,123 @@
+#include "node.h"
+#include <math.h>
+#define PI 3.141592
+#define T_KP 2.58  // P constant for PSD translation controller
+#define T_KD 0.047  // D constant for PSD translation controller
+#define T_KI 0.0  // S constant for PSD translation controller
+#define R_KP 2.0  // P constant for PSD rotation controller
+#define R_KD 0.1  // D constant for PSD rotation controller
+#define R_KI 0.0  // S constant for PSD rotation controller
+
+NodePID::NodePID(ros::Publisher pub, double tol, double tolA, double dist, double ang, double mSpeed, double mASpeed)
+{
+  targetDistance = dist;
+  tolerance = tol;
+  toleranceAngle = tolA;
+  targetAngle = ang;
+  maxSpeed = mSpeed;
+  maxASpeed = mASpeed;  
+  pubMessage = pub;
+  iterations = 0;
+  sumDistance = 0;
+  sumAngle = 0;
+  start = new Point(0.0, 0.0, 0.0, ros::Time::now());
+  last = new Point(0.0, 0.0, 0.0, ros::Time::now());
+}
+
+NodePID::~NodePID()
+{
+}
+
+//Publisher
+void NodePID::publishMessage(double angleCommand, double speedCommand)
+{
+  
+  geometry_msgs::Twist msg;
+
+  msg.linear.x = speedCommand;
+  msg.angular.z = angleCommand;
+
+  //publishing message
+  pubMessage.publish(msg);
+}
+
+//Subscriber
+void NodePID::messageCallback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+  double angleCommand = 0;
+  double speedCommand = 0;
+  Point* actual = new Point(msg->pose.pose.position.x, msg->pose.pose.position.y, 2.0*asin(msg->pose.pose.orientation.z), msg->header.stamp);
+  if (closeEnough(actual) == true)
+  {
+    ROS_INFO("Target found");
+    publishMessage(0.0,0.0);
+    exit(0);
+  }
+  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++;
+
+  
+  if (fabs(targetDistance) > tolerance)
+  {
+    speedCommand = calculatePSD(actual,start->getDistance(actual)*copysign(1.0, targetDistance),start->getDistance(last)*copysign(1.0, targetDistance),targetDistance,T_KP,T_KD,T_KI,&sumDistance);
+  }
+
+  if (actual->angle-last->angle < -PI)
+  {
+    actual->angle += 2*PI;
+  } 
+  else if (actual->angle-last->angle > PI)
+  {
+    actual->angle -= 2*PI;
+  }
+
+  angleCommand = calculatePSD(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;
+
+ 
+  publishMessage(fmin(maxASpeed,angleCommand), fmin(maxSpeed,speedCommand));
+}
+
+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::calculatePSD(MyPoint* actual, double actualValue, double lastValue, double reference, double kP, double kD, double kS, double *sum)
+{
+  double speed = 0;
+  double error = reference - actualValue;
+  double previousError = reference - lastValue;
+  double dt = actual->time.toSec() - last->time.toSec();
+  double derivative = (error - previousError)/dt;
+  *sum = *sum + error*dt;
+  speed = kP*error + kD*derivative + kS*(*sum);
+  return speed;
+}
diff --git a/assignment_2/ajit2704_pid_control/src/pid.cpp b/assignment_2/ajit2704_pid_control/src/pid.cpp
new file mode 100644
index 0000000..e75ba45
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/src/pid.cpp
@@ -0,0 +1,34 @@
+#include "header.h"
+#include "node.h"
+
+#define SUBSCRIBER_SIZE 1  // Size of buffer for subscriber.
+#define PUBLISHER_SIZE 1000  // Size of buffer for publisher.
+#define TOLERANCE 0.01  //at which the distance will be considered as achieved.
+#define TOLERANCE_ANGLE 0.02  // 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 "/cmd_vel"
+#define SUBSCRIBER_TOPIC "odom"
+#define PI 3.141592
+
+int main(int argc, char **argv)
+{
+  //Initialization of node
+  ros::init(argc, argv, "pid");
+  ros::NodeHandle n;
+
+  
+   
+  //Creating publisher
+  ros::Publisher pubMessage = n.advertise<geometry_msgs::Twist>(PUBLISHER_TOPIC, PUBLISHER_BUFFER_SIZE);
+
+  //Creating object, which stores data from sensors and has methods for
+  //publishing and subscribing
+  NodePID *nodePID = new NodePID(pubMessage, TOLERANCE, TOLERANCE_ANGLE, distance, angle, MAX_SPEED, MAX_A_SPEED);
+
+  //Creating subscriber 
+  ros::Subscriber sub = n.subscribe(SUBSCRIBER_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodePID::messageCallback, nodePID);
+  ros::spin();
+
+  return 0;
+}

From 26e75db61dd14ee0687b9123fea8bee1164d32e4 Mon Sep 17 00:00:00 2001
From: ajit2704 <kaleajit27@gmail.com>
Date: Sun, 5 Mar 2017 12:21:46 +0530
Subject: [PATCH 3/5] a

---
 .../ajit2704_pid_control/CMakeLists.txt       |  36 -----
 assignment_2/ajit2704_pid_control/Makefile    |   1 -
 .../ajit2704_pid_control/include/header.h     |  23 ----
 .../ajit2704_pid_control/include/node.h       |  45 -------
 .../ajit2704_pid_control/manifest.xml         |  14 --
 .../ajit2704_pid_control/src/header.cpp       |  23 ----
 .../ajit2704_pid_control/src/node.cpp         | 123 ------------------
 assignment_2/ajit2704_pid_control/src/pid.cpp |  34 -----
 8 files changed, 299 deletions(-)
 delete mode 100644 assignment_2/ajit2704_pid_control/CMakeLists.txt
 delete mode 100644 assignment_2/ajit2704_pid_control/Makefile
 delete mode 100644 assignment_2/ajit2704_pid_control/include/header.h
 delete mode 100644 assignment_2/ajit2704_pid_control/include/node.h
 delete mode 100644 assignment_2/ajit2704_pid_control/manifest.xml
 delete mode 100644 assignment_2/ajit2704_pid_control/src/header.cpp
 delete mode 100644 assignment_2/ajit2704_pid_control/src/node.cpp
 delete mode 100644 assignment_2/ajit2704_pid_control/src/pid.cpp

diff --git a/assignment_2/ajit2704_pid_control/CMakeLists.txt b/assignment_2/ajit2704_pid_control/CMakeLists.txt
deleted file mode 100644
index 5ac565f..0000000
--- a/assignment_2/ajit2704_pid_control/CMakeLists.txt
+++ /dev/null
@@ -1,36 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type.  Options are:
-#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
-#  Debug          : w/ debug symbols, w/o optimization
-#  Release        : w/o debug symbols, w/ optimization
-#  RelWithDebInfo : w/ debug symbols, w/ optimization
-#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
-
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
-set (SRCS1 ${SRCS1} src/Point.cpp) 
-set (SRCS1 ${SRCS1} src/node.cpp) 
-set (SRCS1 ${SRCS1} src/pid.cpp)
-include_directories(include ${catkin_INCLUDE_DIRS})
-add_executable(pid ${SRCS1}) 
-target_link_libraries(pid ${catkin_LIBRARIES}) 
diff --git a/assignment_2/ajit2704_pid_control/Makefile b/assignment_2/ajit2704_pid_control/Makefile
deleted file mode 100644
index b75b928..0000000
--- a/assignment_2/ajit2704_pid_control/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/assignment_2/ajit2704_pid_control/include/header.h b/assignment_2/ajit2704_pid_control/include/header.h
deleted file mode 100644
index 1b1ccff..0000000
--- a/assignment_2/ajit2704_pid_control/include/header.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#include <math.h> 
-#include "ros/ros.h"
-
-class Point
-{
-public:
-
-  
-  Point(double xpos, double ypos, double theta, ros::Time t);
-
-  ~Point();
-
- 
-  double getAngle(Point* target);
-
-  
-  double getDistance(Point* target);
-
-  double x;      
-  double y;      
-  ros::Time time;
-  double angle;  
-};
diff --git a/assignment_2/ajit2704_pid_control/include/node.h b/assignment_2/ajit2704_pid_control/include/node.h
deleted file mode 100644
index ce0b939..0000000
--- a/assignment_2/ajit2704_pid_control/include/node.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#include "header.h"
-#include "ros/ros.h"
-#include "nav_msgs/Odometry.h"
-#include "geometry_msgs/Twist.h"
-#include <queue>
-
-class NodePID
-{
-public:
-
-  
-  NodePID(ros::Publisher pub, double tol, double tolA, double dist, double ang, double mSpeed, double mASpeed);
-
-  ~NodePID();
-
-  
-  void publishMessage(double angleCommand, double speedCommand);
-
- /* This method reads data from sensor and processes them to variables.
-   */
-      
-  void messageCallback(const nav_msgs::Odometry::ConstPtr& msg);
-
-  /*if robot is close enough from target point, target is accomplished and method returns true.
-   */
-  bool closeEnough(Point* actual);
-
-  /* This method calculates action intervention from PSD controller.
-   */
-  double calculatePSD(Point* actual, double actualValue, double lastValue, double reference, double kP, double kD, double kS, double *sum);
-
-//variables
-  Point *start;     // Start position. Distance will be measured from here
-  Point *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 targetDistance; // Robot will go forward this distance.
-  double targetAngle;    // Robot will turn by this angle.
-  int iterations;        // Number of received messages.
-  double sumDistance;    // Sum of distance errors for PSD controller.
-  double sumAngle;       // Sum of angle errors for PSD controller.
-  double toleranceAngle; // Tolerated deviation from target angle.
-};
diff --git a/assignment_2/ajit2704_pid_control/manifest.xml b/assignment_2/ajit2704_pid_control/manifest.xml
deleted file mode 100644
index cadf020..0000000
--- a/assignment_2/ajit2704_pid_control/manifest.xml
+++ /dev/null
@@ -1,14 +0,0 @@
-<package>
-  <description brief="ajit2704_pid_control">
-
-     ajit2704_pid_control
-
-  </description>
-  <author>ajit</author>
-  <license>BSD</license>
-  <review status="unreviewed" notes=""/>
-  <url>http://ros.org/wiki/ajit2704_pid_control</url>
-
-</package>
-
-
diff --git a/assignment_2/ajit2704_pid_control/src/header.cpp b/assignment_2/ajit2704_pid_control/src/header.cpp
deleted file mode 100644
index 70f50b9..0000000
--- a/assignment_2/ajit2704_pid_control/src/header.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-#include "header.h"
-
-Point::Point(double xpos, double ypos, double theta, ros::Time t)
-{
-  x = xpos;
-  y = ypos;
-  angle = theta;
-  time = t;
-}
-
-Point::~Point()
-{
-}
-
-double Point::getAngle(Point* target)
-{
-  return atan2((target->y - y),(target->x - x));
-}
-
-double Point::getDistance(Point* target)
-{
-  return sqrt((pow(target->y - y,2.0)) + (pow(target->x - x,2.0)));
-}
diff --git a/assignment_2/ajit2704_pid_control/src/node.cpp b/assignment_2/ajit2704_pid_control/src/node.cpp
deleted file mode 100644
index 64ec00b..0000000
--- a/assignment_2/ajit2704_pid_control/src/node.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-#include "node.h"
-#include <math.h>
-#define PI 3.141592
-#define T_KP 2.58  // P constant for PSD translation controller
-#define T_KD 0.047  // D constant for PSD translation controller
-#define T_KI 0.0  // S constant for PSD translation controller
-#define R_KP 2.0  // P constant for PSD rotation controller
-#define R_KD 0.1  // D constant for PSD rotation controller
-#define R_KI 0.0  // S constant for PSD rotation controller
-
-NodePID::NodePID(ros::Publisher pub, double tol, double tolA, double dist, double ang, double mSpeed, double mASpeed)
-{
-  targetDistance = dist;
-  tolerance = tol;
-  toleranceAngle = tolA;
-  targetAngle = ang;
-  maxSpeed = mSpeed;
-  maxASpeed = mASpeed;  
-  pubMessage = pub;
-  iterations = 0;
-  sumDistance = 0;
-  sumAngle = 0;
-  start = new Point(0.0, 0.0, 0.0, ros::Time::now());
-  last = new Point(0.0, 0.0, 0.0, ros::Time::now());
-}
-
-NodePID::~NodePID()
-{
-}
-
-//Publisher
-void NodePID::publishMessage(double angleCommand, double speedCommand)
-{
-  
-  geometry_msgs::Twist msg;
-
-  msg.linear.x = speedCommand;
-  msg.angular.z = angleCommand;
-
-  //publishing message
-  pubMessage.publish(msg);
-}
-
-//Subscriber
-void NodePID::messageCallback(const nav_msgs::Odometry::ConstPtr& msg)
-{
-  double angleCommand = 0;
-  double speedCommand = 0;
-  Point* actual = new Point(msg->pose.pose.position.x, msg->pose.pose.position.y, 2.0*asin(msg->pose.pose.orientation.z), msg->header.stamp);
-  if (closeEnough(actual) == true)
-  {
-    ROS_INFO("Target found");
-    publishMessage(0.0,0.0);
-    exit(0);
-  }
-  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++;
-
-  
-  if (fabs(targetDistance) > tolerance)
-  {
-    speedCommand = calculatePSD(actual,start->getDistance(actual)*copysign(1.0, targetDistance),start->getDistance(last)*copysign(1.0, targetDistance),targetDistance,T_KP,T_KD,T_KI,&sumDistance);
-  }
-
-  if (actual->angle-last->angle < -PI)
-  {
-    actual->angle += 2*PI;
-  } 
-  else if (actual->angle-last->angle > PI)
-  {
-    actual->angle -= 2*PI;
-  }
-
-  angleCommand = calculatePSD(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;
-
- 
-  publishMessage(fmin(maxASpeed,angleCommand), fmin(maxSpeed,speedCommand));
-}
-
-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::calculatePSD(MyPoint* actual, double actualValue, double lastValue, double reference, double kP, double kD, double kS, double *sum)
-{
-  double speed = 0;
-  double error = reference - actualValue;
-  double previousError = reference - lastValue;
-  double dt = actual->time.toSec() - last->time.toSec();
-  double derivative = (error - previousError)/dt;
-  *sum = *sum + error*dt;
-  speed = kP*error + kD*derivative + kS*(*sum);
-  return speed;
-}
diff --git a/assignment_2/ajit2704_pid_control/src/pid.cpp b/assignment_2/ajit2704_pid_control/src/pid.cpp
deleted file mode 100644
index e75ba45..0000000
--- a/assignment_2/ajit2704_pid_control/src/pid.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-#include "header.h"
-#include "node.h"
-
-#define SUBSCRIBER_SIZE 1  // Size of buffer for subscriber.
-#define PUBLISHER_SIZE 1000  // Size of buffer for publisher.
-#define TOLERANCE 0.01  //at which the distance will be considered as achieved.
-#define TOLERANCE_ANGLE 0.02  // 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 "/cmd_vel"
-#define SUBSCRIBER_TOPIC "odom"
-#define PI 3.141592
-
-int main(int argc, char **argv)
-{
-  //Initialization of node
-  ros::init(argc, argv, "pid");
-  ros::NodeHandle n;
-
-  
-   
-  //Creating publisher
-  ros::Publisher pubMessage = n.advertise<geometry_msgs::Twist>(PUBLISHER_TOPIC, PUBLISHER_BUFFER_SIZE);
-
-  //Creating object, which stores data from sensors and has methods for
-  //publishing and subscribing
-  NodePID *nodePID = new NodePID(pubMessage, TOLERANCE, TOLERANCE_ANGLE, distance, angle, MAX_SPEED, MAX_A_SPEED);
-
-  //Creating subscriber 
-  ros::Subscriber sub = n.subscribe(SUBSCRIBER_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodePID::messageCallback, nodePID);
-  ros::spin();
-
-  return 0;
-}

From 941e3a31f4ce4f2522d0b68a2e3192ef563acd79 Mon Sep 17 00:00:00 2001
From: ajit2704 <kaleajit27@gmail.com>
Date: Sun, 5 Mar 2017 12:50:41 +0530
Subject: [PATCH 4/5] assignment2

---
 .../ajit2704_pid_control/CMakeLists.txt       |  36 +++++
 assignment_2/ajit2704_pid_control/Makefile    |   1 +
 .../ajit2704_pid_control/include/header.h     |  23 ++++
 .../ajit2704_pid_control/include/node.h       |  45 +++++++
 .../ajit2704_pid_control/manifest.xml         |  14 ++
 .../ajit2704_pid_control/src/header.cpp       |  23 ++++
 .../ajit2704_pid_control/src/node.cpp         | 123 ++++++++++++++++++
 assignment_2/ajit2704_pid_control/src/pid.cpp |  34 +++++
 8 files changed, 299 insertions(+)
 create mode 100644 assignment_2/ajit2704_pid_control/CMakeLists.txt
 create mode 100644 assignment_2/ajit2704_pid_control/Makefile
 create mode 100644 assignment_2/ajit2704_pid_control/include/header.h
 create mode 100644 assignment_2/ajit2704_pid_control/include/node.h
 create mode 100644 assignment_2/ajit2704_pid_control/manifest.xml
 create mode 100644 assignment_2/ajit2704_pid_control/src/header.cpp
 create mode 100644 assignment_2/ajit2704_pid_control/src/node.cpp
 create mode 100644 assignment_2/ajit2704_pid_control/src/pid.cpp

diff --git a/assignment_2/ajit2704_pid_control/CMakeLists.txt b/assignment_2/ajit2704_pid_control/CMakeLists.txt
new file mode 100644
index 0000000..5ac565f
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/CMakeLists.txt
@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+set (SRCS1 ${SRCS1} src/Point.cpp) 
+set (SRCS1 ${SRCS1} src/node.cpp) 
+set (SRCS1 ${SRCS1} src/pid.cpp)
+include_directories(include ${catkin_INCLUDE_DIRS})
+add_executable(pid ${SRCS1}) 
+target_link_libraries(pid ${catkin_LIBRARIES}) 
diff --git a/assignment_2/ajit2704_pid_control/Makefile b/assignment_2/ajit2704_pid_control/Makefile
new file mode 100644
index 0000000..b75b928
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/Makefile
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/assignment_2/ajit2704_pid_control/include/header.h b/assignment_2/ajit2704_pid_control/include/header.h
new file mode 100644
index 0000000..1b1ccff
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/include/header.h
@@ -0,0 +1,23 @@
+#include <math.h> 
+#include "ros/ros.h"
+
+class Point
+{
+public:
+
+  
+  Point(double xpos, double ypos, double theta, ros::Time t);
+
+  ~Point();
+
+ 
+  double getAngle(Point* target);
+
+  
+  double getDistance(Point* target);
+
+  double x;      
+  double y;      
+  ros::Time time;
+  double angle;  
+};
diff --git a/assignment_2/ajit2704_pid_control/include/node.h b/assignment_2/ajit2704_pid_control/include/node.h
new file mode 100644
index 0000000..ce0b939
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/include/node.h
@@ -0,0 +1,45 @@
+#include "header.h"
+#include "ros/ros.h"
+#include "nav_msgs/Odometry.h"
+#include "geometry_msgs/Twist.h"
+#include <queue>
+
+class NodePID
+{
+public:
+
+  
+  NodePID(ros::Publisher pub, double tol, double tolA, double dist, double ang, double mSpeed, double mASpeed);
+
+  ~NodePID();
+
+  
+  void publishMessage(double angleCommand, double speedCommand);
+
+ /* This method reads data from sensor and processes them to variables.
+   */
+      
+  void messageCallback(const nav_msgs::Odometry::ConstPtr& msg);
+
+  /*if robot is close enough from target point, target is accomplished and method returns true.
+   */
+  bool closeEnough(Point* actual);
+
+  /* This method calculates action intervention from PSD controller.
+   */
+  double calculatePSD(Point* actual, double actualValue, double lastValue, double reference, double kP, double kD, double kS, double *sum);
+
+//variables
+  Point *start;     // Start position. Distance will be measured from here
+  Point *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 targetDistance; // Robot will go forward this distance.
+  double targetAngle;    // Robot will turn by this angle.
+  int iterations;        // Number of received messages.
+  double sumDistance;    // Sum of distance errors for PSD controller.
+  double sumAngle;       // Sum of angle errors for PSD controller.
+  double toleranceAngle; // Tolerated deviation from target angle.
+};
diff --git a/assignment_2/ajit2704_pid_control/manifest.xml b/assignment_2/ajit2704_pid_control/manifest.xml
new file mode 100644
index 0000000..cadf020
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/manifest.xml
@@ -0,0 +1,14 @@
+<package>
+  <description brief="ajit2704_pid_control">
+
+     ajit2704_pid_control
+
+  </description>
+  <author>ajit</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/ajit2704_pid_control</url>
+
+</package>
+
+
diff --git a/assignment_2/ajit2704_pid_control/src/header.cpp b/assignment_2/ajit2704_pid_control/src/header.cpp
new file mode 100644
index 0000000..70f50b9
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/src/header.cpp
@@ -0,0 +1,23 @@
+#include "header.h"
+
+Point::Point(double xpos, double ypos, double theta, ros::Time t)
+{
+  x = xpos;
+  y = ypos;
+  angle = theta;
+  time = t;
+}
+
+Point::~Point()
+{
+}
+
+double Point::getAngle(Point* target)
+{
+  return atan2((target->y - y),(target->x - x));
+}
+
+double Point::getDistance(Point* target)
+{
+  return sqrt((pow(target->y - y,2.0)) + (pow(target->x - x,2.0)));
+}
diff --git a/assignment_2/ajit2704_pid_control/src/node.cpp b/assignment_2/ajit2704_pid_control/src/node.cpp
new file mode 100644
index 0000000..64ec00b
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/src/node.cpp
@@ -0,0 +1,123 @@
+#include "node.h"
+#include <math.h>
+#define PI 3.141592
+#define T_KP 2.58  // P constant for PSD translation controller
+#define T_KD 0.047  // D constant for PSD translation controller
+#define T_KI 0.0  // S constant for PSD translation controller
+#define R_KP 2.0  // P constant for PSD rotation controller
+#define R_KD 0.1  // D constant for PSD rotation controller
+#define R_KI 0.0  // S constant for PSD rotation controller
+
+NodePID::NodePID(ros::Publisher pub, double tol, double tolA, double dist, double ang, double mSpeed, double mASpeed)
+{
+  targetDistance = dist;
+  tolerance = tol;
+  toleranceAngle = tolA;
+  targetAngle = ang;
+  maxSpeed = mSpeed;
+  maxASpeed = mASpeed;  
+  pubMessage = pub;
+  iterations = 0;
+  sumDistance = 0;
+  sumAngle = 0;
+  start = new Point(0.0, 0.0, 0.0, ros::Time::now());
+  last = new Point(0.0, 0.0, 0.0, ros::Time::now());
+}
+
+NodePID::~NodePID()
+{
+}
+
+//Publisher
+void NodePID::publishMessage(double angleCommand, double speedCommand)
+{
+  
+  geometry_msgs::Twist msg;
+
+  msg.linear.x = speedCommand;
+  msg.angular.z = angleCommand;
+
+  //publishing message
+  pubMessage.publish(msg);
+}
+
+//Subscriber
+void NodePID::messageCallback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+  double angleCommand = 0;
+  double speedCommand = 0;
+  Point* actual = new Point(msg->pose.pose.position.x, msg->pose.pose.position.y, 2.0*asin(msg->pose.pose.orientation.z), msg->header.stamp);
+  if (closeEnough(actual) == true)
+  {
+    ROS_INFO("Target found");
+    publishMessage(0.0,0.0);
+    exit(0);
+  }
+  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++;
+
+  
+  if (fabs(targetDistance) > tolerance)
+  {
+    speedCommand = calculatePSD(actual,start->getDistance(actual)*copysign(1.0, targetDistance),start->getDistance(last)*copysign(1.0, targetDistance),targetDistance,T_KP,T_KD,T_KI,&sumDistance);
+  }
+
+  if (actual->angle-last->angle < -PI)
+  {
+    actual->angle += 2*PI;
+  } 
+  else if (actual->angle-last->angle > PI)
+  {
+    actual->angle -= 2*PI;
+  }
+
+  angleCommand = calculatePSD(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;
+
+ 
+  publishMessage(fmin(maxASpeed,angleCommand), fmin(maxSpeed,speedCommand));
+}
+
+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::calculatePSD(MyPoint* actual, double actualValue, double lastValue, double reference, double kP, double kD, double kS, double *sum)
+{
+  double speed = 0;
+  double error = reference - actualValue;
+  double previousError = reference - lastValue;
+  double dt = actual->time.toSec() - last->time.toSec();
+  double derivative = (error - previousError)/dt;
+  *sum = *sum + error*dt;
+  speed = kP*error + kD*derivative + kS*(*sum);
+  return speed;
+}
diff --git a/assignment_2/ajit2704_pid_control/src/pid.cpp b/assignment_2/ajit2704_pid_control/src/pid.cpp
new file mode 100644
index 0000000..e75ba45
--- /dev/null
+++ b/assignment_2/ajit2704_pid_control/src/pid.cpp
@@ -0,0 +1,34 @@
+#include "header.h"
+#include "node.h"
+
+#define SUBSCRIBER_SIZE 1  // Size of buffer for subscriber.
+#define PUBLISHER_SIZE 1000  // Size of buffer for publisher.
+#define TOLERANCE 0.01  //at which the distance will be considered as achieved.
+#define TOLERANCE_ANGLE 0.02  // 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 "/cmd_vel"
+#define SUBSCRIBER_TOPIC "odom"
+#define PI 3.141592
+
+int main(int argc, char **argv)
+{
+  //Initialization of node
+  ros::init(argc, argv, "pid");
+  ros::NodeHandle n;
+
+  
+   
+  //Creating publisher
+  ros::Publisher pubMessage = n.advertise<geometry_msgs::Twist>(PUBLISHER_TOPIC, PUBLISHER_BUFFER_SIZE);
+
+  //Creating object, which stores data from sensors and has methods for
+  //publishing and subscribing
+  NodePID *nodePID = new NodePID(pubMessage, TOLERANCE, TOLERANCE_ANGLE, distance, angle, MAX_SPEED, MAX_A_SPEED);
+
+  //Creating subscriber 
+  ros::Subscriber sub = n.subscribe(SUBSCRIBER_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodePID::messageCallback, nodePID);
+  ros::spin();
+
+  return 0;
+}

From 6cbb8985cdd9dfb6a69e2b51590700be1965e7cc Mon Sep 17 00:00:00 2001
From: ajit2704 <kaleajit27@gmail.com>
Date: Sun, 5 Mar 2017 12:55:49 +0530
Subject: [PATCH 5/5] change

---
 assignment_1/ajit2704_print_squares | 1 -
 1 file changed, 1 deletion(-)
 delete mode 160000 assignment_1/ajit2704_print_squares

diff --git a/assignment_1/ajit2704_print_squares b/assignment_1/ajit2704_print_squares
deleted file mode 160000
index 668d65d..0000000
--- a/assignment_1/ajit2704_print_squares
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 668d65ded2da52e87abcafc8c9b85e0317b0987d