From 30836fade7b90557ee768b23a5b9739a084925fe Mon Sep 17 00:00:00 2001 From: ajit2704 Date: Sun, 5 Mar 2017 01:15:31 +0530 Subject: [PATCH 1/3] 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 Date: Sun, 5 Mar 2017 12:13:42 +0530 Subject: [PATCH 2/3] 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 +#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 + +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 @@ + + + + ajit2704_pid_control + + + ajit + BSD + + http://ros.org/wiki/ajit2704_pid_control + + + + 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 +#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(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 Date: Sun, 5 Mar 2017 12:21:46 +0530 Subject: [PATCH 3/3] 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 -#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 - -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 @@ - - - - ajit2704_pid_control - - - ajit - BSD - - http://ros.org/wiki/ajit2704_pid_control - - - - 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 -#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(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; -}