diff --git a/src/pid b/src/pid deleted file mode 160000 index 8268b72..0000000 --- a/src/pid +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8268b7250e40c33a08d7faa7706885eb317eadbd diff --git a/src/pid/CHANGELOG.rst b/src/pid/CHANGELOG.rst new file mode 100644 index 0000000..24f99ce --- /dev/null +++ b/src/pid/CHANGELOG.rst @@ -0,0 +1,89 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pid +^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.28 (2020-07-03) +------------------- +* Fix compatibility issues when building with ROS Noetic +* Bug fix for angle wrapping always resetting filtered and integral error +* Adding a parameter setpoint_timeout which specifies whether to keep publishing control_effort messages after setpoint messages stop coming in +* Add link to low-pass filter reference +* Cleaning up package.xml just a bit more. +* Removing a bunch of superfluous dependencies. +* Cleaning CMakeLists. +* Upgrade to package format=2 +* Contributors: Andrew J Zelenak, AndyZe, Stewart Jamieson, Vikrant Shah + +0.0.15 (2016-02-22) +------------------- +* Fixing header "include" error, possible dynamic_reconfigure errors. +* Contributors: Andy Zelenak + +0.0.10 (2016-1-25) +------------------ +* Add diagnostics running at 4Hz. Publish interesting data like setpoint, plant state, +error, control effort in diags for viewing +* Add ROS private parameters to set Pid params and simulator param +* Add Auto/Manual mode: Listen to the /pid_enable topic for a std_msgs/Bool +that will disable or re-enable output from the PID controller +* Support reverse_acting parameter, for plants where the control input acts +in the opposite direction to the plant-state. E.g. Differential-drive robots +often have one direct and one reverse-acting PID +* Support faster-than-wallclock simulation via a /clock publisher and the +/use_sim_time parameter +* Add launch files for first & 2nd order behaviors. Get them to launch plots, +diag monitor, reconfigure gui +* Split setpoint generator out into a separate node +* Rename simulator to plant_sim.cpp & give it 1st & 2nd order behaviors, configured +with a parameter. +* Delete first_order_plant_sim.cpp, which is subsumed into plant_sim.cpp. Remove +plant header, which was almost content-free. +* Remove msg directory & switch to using Float64 messages for setpoint, +process state, & control effort. Now it's generic - no special messages needed. +* Explicitly call out std namespace to avoid accidental name conflicts +* Remove parameter length checks to allow parameters to be set in part from +launch file (the usual way), and in part from cmd line args (an infrequently-used +way) +* Rename pid_header.h to controller.h because it's used by controller.cpp - more standard +C++ naming style +* Add copyrights +* Contributors: AndyZe, Paul Bouchier + +0.0.9 (2015-12-27) +------------------ +* Merged in bouchier/pid (pull request #1) + Add dynamic reconfigure for Kx, change plant setpoint +* add dynamic reconfigure for Kx, change plant setpoint +* Contributors: AndyZe, Paul Bouchier + +0.0.7 (2015-07-26) +------------------ +* Added a launch file, which required the arguments to be processed differently. +* Contributors: Andy Zelenak + +0.0.6 (2015-06-09) +------------------ +* Changing the way parameters are passed to check_user_input(). +* Contributors: Andy Zelenak + +0.0.5 (2015-06-09) +------------------ +* Adding an anti-windup option. +* Contributors: Andy Zelenak + +0.0.3 (2015-03-14) +------------------ + +0.0.2 (2015-03-13) +------------------ + +0.0.1 (2015-03-08) +------------------ +* Fixing various minor bugs related to user input. +* Pre-release commit. +* It WORKS! +* It's talking with the plant sim now. Just the PID programming remains. +* Making progress with the command line input. +* Rough outline of the program. Need to take inputs on the command line next. +* Initial commit +* Contributors: Andy Zelenak diff --git a/src/pid/CMakeLists.txt b/src/pid/CMakeLists.txt new file mode 100644 index 0000000..2fbeae5 --- /dev/null +++ b/src/pid/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pid) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + roscpp + dynamic_reconfigure +) +find_package(Boost REQUIRED COMPONENTS system) + +generate_dynamic_reconfigure_options( + cfg/Pid.cfg +) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp std_msgs message_runtime dynamic_reconfigure +) + +include_directories(include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake +) + +add_executable(controller src/controller.cpp src/pid.cpp) +add_executable(plant_sim src/plant_sim.cpp) +add_executable(setpoint_node src/setpoint_node.cpp) +add_executable(sim_time src/sim_time.cpp) +add_executable(autotune src/autotune.cpp) + +add_dependencies(controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(plant_sim ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(setpoint_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(sim_time ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(autotune ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(controller ${catkin_LIBRARIES}) +target_link_libraries(plant_sim ${catkin_LIBRARIES}) +target_link_libraries(setpoint_node ${catkin_LIBRARIES}) +target_link_libraries(sim_time ${catkin_LIBRARIES}) +target_link_libraries(autotune ${catkin_LIBRARIES}) + +install(TARGETS controller plant_sim setpoint_node sim_time autotune + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/src/pid/cfg/Pid.cfg b/src/pid/cfg/Pid.cfg new file mode 100644 index 0000000..9466710 --- /dev/null +++ b/src/pid/cfg/Pid.cfg @@ -0,0 +1,20 @@ +#!/usr/bin/env python +PACKAGE = "pid" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() +K_scale_enum = gen.enum([ gen.const("scale_tenth", double_t, 0.1, "Scale by 0.1"), + gen.const("scale_unity", double_t, 1.0, "No scaling"), + gen.const("scale_ten", double_t, 10.0, "Scale by 10"), + gen.const("scale_hundred", double_t, 100.0, "Scale by 100")], + "Scale factor for K setting") + +gen.add("Kp_scale", double_t, 0, "Kp scale", 10, 0.1, 100, edit_method = K_scale_enum) +gen.add("Kp", double_t, 0, "Kp", 0.1, -1, 1) +gen.add("Ki_scale", double_t, 0, "Ki scale", 10, 0.1, 100, edit_method = K_scale_enum) +gen.add("Ki", double_t, 0, "Ki", 0.1, -1, 1) +gen.add("Kd_scale", double_t, 0, "Kd scale", 10, 0.1, 100, edit_method = K_scale_enum) +gen.add("Kd", double_t, 0, "Kd", 0.1, -1, 1) + +exit(gen.generate(PACKAGE, "pid", "Pid")) diff --git a/src/pid/contributors.txt b/src/pid/contributors.txt new file mode 100644 index 0000000..9569179 --- /dev/null +++ b/src/pid/contributors.txt @@ -0,0 +1,2 @@ +Andy Zelenak +Paul Bouchier diff --git a/src/pid/include/pid/PidConfig.h b/src/pid/include/pid/PidConfig.h new file mode 100644 index 0000000..d69acab --- /dev/null +++ b/src/pid/include/pid/PidConfig.h @@ -0,0 +1,736 @@ +//#line 2 +//"/opt/ros/indigo/share/dynamic_reconfigure/cmake/../templates/ConfigType.h.template" +// ********************************************************* +// +// File autogenerated for the pid package +// by the dynamic_reconfigure package. +// Please do not edit. +// +// ********************************************************/ + +#include +#include +#include + +#ifndef __pid__PIDCONFIG_H__ +#define __pid__PIDCONFIG_H__ + +namespace pid +{ +class PidConfigStatics; + +class PidConfig +{ +public: + class AbstractParamDescription : public dynamic_reconfigure::ParamDescription + { + public: + AbstractParamDescription(std::string n, std::string t, uint32_t l, std::string d, std::string e) + { + name = n; + type = t; + level = l; + description = d; + edit_method = e; + } + + virtual void clamp(PidConfig& config, const PidConfig& max, const PidConfig& min) const = 0; + virtual void calcLevel(uint32_t& level, const PidConfig& config1, const PidConfig& config2) const = 0; + virtual void fromServer(const ros::NodeHandle& nh, PidConfig& config) const = 0; + virtual void toServer(const ros::NodeHandle& nh, const PidConfig& config) const = 0; + virtual bool fromMessage(const dynamic_reconfigure::Config& msg, PidConfig& config) const = 0; + virtual void toMessage(dynamic_reconfigure::Config& msg, const PidConfig& config) const = 0; + virtual void getValue(const PidConfig& config, boost::any& val) const = 0; + }; + + typedef boost::shared_ptr AbstractParamDescriptionPtr; + typedef boost::shared_ptr AbstractParamDescriptionConstPtr; + + template + class ParamDescription : public AbstractParamDescription + { + public: + ParamDescription(std::string name, std::string type, uint32_t level, std::string description, + std::string edit_method, T PidConfig::*f) + : AbstractParamDescription(name, type, level, description, edit_method), field(f) + { + } + + T(PidConfig::*field); + + virtual void clamp(PidConfig& config, const PidConfig& max, const PidConfig& min) const + { + if (config.*field > max.*field) + config.*field = max.*field; + + if (config.*field < min.*field) + config.*field = min.*field; + } + + virtual void calcLevel(uint32_t& comb_level, const PidConfig& config1, const PidConfig& config2) const + { + if (config1.*field != config2.*field) + comb_level |= level; + } + + virtual void fromServer(const ros::NodeHandle& nh, PidConfig& config) const + { + nh.getParam(name, config.*field); + } + + virtual void toServer(const ros::NodeHandle& nh, const PidConfig& config) const + { + nh.setParam(name, config.*field); + } + + virtual bool fromMessage(const dynamic_reconfigure::Config& msg, PidConfig& config) const + { + return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*field); + } + + virtual void toMessage(dynamic_reconfigure::Config& msg, const PidConfig& config) const + { + dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*field); + } + + virtual void getValue(const PidConfig& config, boost::any& val) const + { + val = config.*field; + } + }; + + class AbstractGroupDescription : public dynamic_reconfigure::Group + { + public: + AbstractGroupDescription(std::string n, std::string t, int p, int i, bool s) + { + name = n; + type = t; + parent = p; + state = s; + id = i; + } + + std::vector abstract_parameters; + bool state; + + virtual void toMessage(dynamic_reconfigure::Config& msg, const boost::any& config) const = 0; + virtual bool fromMessage(const dynamic_reconfigure::Config& msg, boost::any& config) const = 0; + virtual void updateParams(boost::any& cfg, PidConfig& top) const = 0; + virtual void setInitialState(boost::any& cfg) const = 0; + + void convertParams() + { + for (std::vector::const_iterator i = abstract_parameters.begin(); + i != abstract_parameters.end(); ++i) + { + parameters.push_back(dynamic_reconfigure::ParamDescription(**i)); + } + } + }; + + typedef boost::shared_ptr AbstractGroupDescriptionPtr; + typedef boost::shared_ptr AbstractGroupDescriptionConstPtr; + + template + class GroupDescription : public AbstractGroupDescription + { + public: + GroupDescription(std::string name, std::string type, int parent, int id, bool s, T PT::*f) + : AbstractGroupDescription(name, type, parent, id, s), field(f) + { + } + + GroupDescription(const GroupDescription& g) + : AbstractGroupDescription(g.name, g.type, g.parent, g.id, g.state), field(g.field), groups(g.groups) + { + parameters = g.parameters; + abstract_parameters = g.abstract_parameters; + } + + virtual bool fromMessage(const dynamic_reconfigure::Config& msg, boost::any& cfg) const + { + PT* config = boost::any_cast(cfg); + if (!dynamic_reconfigure::ConfigTools::getGroupState(msg, name, (*config).*field)) + return false; + + for (std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + boost::any n = &((*config).*field); + if (!(*i)->fromMessage(msg, n)) + return false; + } + + return true; + } + + virtual void setInitialState(boost::any& cfg) const + { + PT* config = boost::any_cast(cfg); + T* group = &((*config).*field); + group->state = state; + + for (std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + boost::any n = boost::any(&((*config).*field)); + (*i)->setInitialState(n); + } + } + + virtual void updateParams(boost::any& cfg, PidConfig& top) const + { + PT* config = boost::any_cast(cfg); + + T* f = &((*config).*field); + f->setParams(top, abstract_parameters); + + for (std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + boost::any n = &((*config).*field); + (*i)->updateParams(n, top); + } + } + + virtual void toMessage(dynamic_reconfigure::Config& msg, const boost::any& cfg) const + { + const PT config = boost::any_cast(cfg); + dynamic_reconfigure::ConfigTools::appendGroup(msg, name, id, parent, config.*field); + + for (std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + (*i)->toMessage(msg, config.*field); + } + } + + T(PT::*field); + std::vector groups; + }; + + class DEFAULT + { + public: + DEFAULT() + { + state = true; + name = "Default"; + } + + void setParams(PidConfig& config, const std::vector params) + { + for (std::vector::const_iterator _i = params.begin(); _i != params.end(); ++_i) + { + boost::any val; + (*_i)->getValue(config, val); + + if ("Kp_scale" == (*_i)->name) + { + Kp_scale = boost::any_cast(val); + } + if ("Kp" == (*_i)->name) + { + Kp = boost::any_cast(val); + } + if ("Ki_scale" == (*_i)->name) + { + Ki_scale = boost::any_cast(val); + } + if ("Ki" == (*_i)->name) + { + Ki = boost::any_cast(val); + } + if ("Kd_scale" == (*_i)->name) + { + Kd_scale = boost::any_cast(val); + } + if ("Kd" == (*_i)->name) + { + Kd = boost::any_cast(val); + } + } + } + + double Kp_scale; + double Kp; + double Ki_scale; + double Ki; + double Kd_scale; + double Kd; + + bool state; + std::string name; + + } groups; + + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Kp_scale; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Kp; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Ki_scale; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Ki; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Kd_scale; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Kd; + //#line 218 + //"/opt/ros/indigo/share/dynamic_reconfigure/cmake/../templates/ConfigType.h.template" + + bool __fromMessage__(dynamic_reconfigure::Config& msg) + { + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + const std::vector& __group_descriptions__ = __getGroupDescriptions__(); + + int count = 0; + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + if ((*i)->fromMessage(msg, *this)) + count++; + + for (std::vector::const_iterator i = __group_descriptions__.begin(); + i != __group_descriptions__.end(); i++) + { + if ((*i)->id == 0) + { + boost::any n = boost::any(this); + (*i)->updateParams(n, *this); + (*i)->fromMessage(msg, n); + } + } + + if (count != dynamic_reconfigure::ConfigTools::size(msg)) + { + ROS_ERROR("PidConfig::__fromMessage__ called with an unexpected parameter."); + ROS_ERROR("Booleans:"); + for (unsigned int i = 0; i < msg.bools.size(); i++) + ROS_ERROR(" %s", msg.bools[i].name.c_str()); + ROS_ERROR("Integers:"); + for (unsigned int i = 0; i < msg.ints.size(); i++) + ROS_ERROR(" %s", msg.ints[i].name.c_str()); + ROS_ERROR("Doubles:"); + for (unsigned int i = 0; i < msg.doubles.size(); i++) + ROS_ERROR(" %s", msg.doubles[i].name.c_str()); + ROS_ERROR("Strings:"); + for (unsigned int i = 0; i < msg.strs.size(); i++) + ROS_ERROR(" %s", msg.strs[i].name.c_str()); + // @todo Check that there are no duplicates. Make this error more + // explicit. + return false; + } + return true; + } + + // This version of __toMessage__ is used during initialization of + // statics when __getParamDescriptions__ can't be called yet. + void __toMessage__(dynamic_reconfigure::Config& msg, + const std::vector& __param_descriptions__, + const std::vector& __group_descriptions__) const + { + dynamic_reconfigure::ConfigTools::clear(msg); + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + (*i)->toMessage(msg, *this); + + for (std::vector::const_iterator i = __group_descriptions__.begin(); + i != __group_descriptions__.end(); ++i) + { + if ((*i)->id == 0) + { + (*i)->toMessage(msg, *this); + } + } + } + + void __toMessage__(dynamic_reconfigure::Config& msg) const + { + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + const std::vector& __group_descriptions__ = __getGroupDescriptions__(); + __toMessage__(msg, __param_descriptions__, __group_descriptions__); + } + + void __toServer__(const ros::NodeHandle& nh) const + { + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + (*i)->toServer(nh, *this); + } + + void __fromServer__(const ros::NodeHandle& nh) + { + static bool setup = false; + + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + (*i)->fromServer(nh, *this); + + const std::vector& __group_descriptions__ = __getGroupDescriptions__(); + for (std::vector::const_iterator i = __group_descriptions__.begin(); + i != __group_descriptions__.end(); i++) + { + if (!setup && (*i)->id == 0) + { + setup = true; + boost::any n = boost::any(this); + (*i)->setInitialState(n); + } + } + } + + void __clamp__() + { + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + const PidConfig& __max__ = __getMax__(); + const PidConfig& __min__ = __getMin__(); + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + (*i)->clamp(*this, __max__, __min__); + } + + uint32_t __level__(const PidConfig& config) const + { + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + uint32_t level = 0; + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + (*i)->calcLevel(level, config, *this); + return level; + } + + static const dynamic_reconfigure::ConfigDescription& __getDescriptionMessage__(); + static const PidConfig& __getDefault__(); + static const PidConfig& __getMax__(); + static const PidConfig& __getMin__(); + static const std::vector& __getParamDescriptions__(); + static const std::vector& __getGroupDescriptions__(); + +private: + static const PidConfigStatics* __get_statics__(); +}; + +template <> // Max and min are ignored for strings. +inline void PidConfig::ParamDescription::clamp(PidConfig& config, const PidConfig& max, + const PidConfig& min) const +{ + return; +} + +class PidConfigStatics +{ + friend class PidConfig; + + PidConfigStatics() + { + PidConfig::GroupDescription Default("Default", "", 0, 0, true, &PidConfig::groups); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Kp_scale = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Kp_scale = 100.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Kp_scale = 10.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kp_scale", "double", 0, "Kp scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Kp_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kp_scale", "double", 0, "Kp scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Kp_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Kp = 0.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Kp = 1.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Kp = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kp", "double", 0, "Kp", "", &PidConfig::Kp))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kp", "double", 0, "Kp", "", &PidConfig::Kp))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Ki_scale = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Ki_scale = 100.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Ki_scale = 10.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Ki_scale", "double", 0, "Ki scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Ki_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Ki_scale", "double", 0, "Ki scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Ki_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Ki = 0.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Ki = 1.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Ki = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Ki", "double", 0, "Ki", "", &PidConfig::Ki))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Ki", "double", 0, "Ki", "", &PidConfig::Ki))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Kd_scale = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Kd_scale = 100.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Kd_scale = 10.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kd_scale", "double", 0, "Kd scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Kd_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kd_scale", "double", 0, "Kd scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Kd_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Kd = 0.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Kd = 1.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Kd = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kd", "double", 0, "Kd", "", &PidConfig::Kd))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kd", "double", 0, "Kd", "", &PidConfig::Kd))); + //#line 233 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.convertParams(); + //#line 233 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __group_descriptions__.push_back(PidConfig::AbstractGroupDescriptionConstPtr( + new PidConfig::GroupDescription(Default))); + //#line 353 + //"/opt/ros/indigo/share/dynamic_reconfigure/cmake/../templates/ConfigType.h.template" + + for (std::vector::const_iterator i = __group_descriptions__.begin(); + i != __group_descriptions__.end(); ++i) + { + __description_message__.groups.push_back(**i); + } + __max__.__toMessage__(__description_message__.max, __param_descriptions__, __group_descriptions__); + __min__.__toMessage__(__description_message__.min, __param_descriptions__, __group_descriptions__); + __default__.__toMessage__(__description_message__.dflt, __param_descriptions__, __group_descriptions__); + } + std::vector __param_descriptions__; + std::vector __group_descriptions__; + PidConfig __max__; + PidConfig __min__; + PidConfig __default__; + dynamic_reconfigure::ConfigDescription __description_message__; + + static const PidConfigStatics* get_instance() + { + // Split this off in a separate function because I know that + // instance will get initialized the first time get_instance is + // called, and I am guaranteeing that get_instance gets called at + // most once. + static PidConfigStatics instance; + return &instance; + } +}; + +inline const dynamic_reconfigure::ConfigDescription& PidConfig::__getDescriptionMessage__() +{ + return __get_statics__()->__description_message__; +} + +inline const PidConfig& PidConfig::__getDefault__() +{ + return __get_statics__()->__default__; +} + +inline const PidConfig& PidConfig::__getMax__() +{ + return __get_statics__()->__max__; +} + +inline const PidConfig& PidConfig::__getMin__() +{ + return __get_statics__()->__min__; +} + +inline const std::vector& PidConfig::__getParamDescriptions__() +{ + return __get_statics__()->__param_descriptions__; +} + +inline const std::vector& PidConfig::__getGroupDescriptions__() +{ + return __get_statics__()->__group_descriptions__; +} + +inline const PidConfigStatics* PidConfig::__get_statics__() +{ + const static PidConfigStatics* statics; + + if (statics) // Common case + return statics; + + boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__); + + if (statics) // In case we lost a race. + return statics; + + statics = PidConfigStatics::get_instance(); + + return statics; +} + +//#line 7 "/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg" +const double Pid_scale_tenth = 0.1; +//#line 8 "/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg" +const double Pid_scale_unity = 1.0; +//#line 9 "/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg" +const double Pid_scale_ten = 10.0; +//#line 10 "/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg" +const double Pid_scale_hundred = 100.0; +} + +#endif // __PIDRECONFIGURATOR_H__ diff --git a/src/pid/include/pid/controller.h b/src/pid/include/pid/controller.h new file mode 100644 index 0000000..397ddb5 --- /dev/null +++ b/src/pid/include/pid/controller.h @@ -0,0 +1,112 @@ +/***************************************************************************/ /** + * \file controller.h + * + * \brief Simple PID controller with dynamic reconfigure + * \author Andy Zelenak + * \date March 8, 2015 + * + * \section license License (BSD-3) + * Copyright (c) 2015, Andy Zelenak\n + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#ifndef CONTROLLER_H +#define CONTROLLER_H + +namespace pid_ns +{ +class PidObject +{ +public: + PidObject(); + +private: + void doCalcs(); + void getParams(double in, double& value, double& scale); + void pidEnableCallback(const std_msgs::Bool& pid_enable_msg); + void plantStateCallback(const std_msgs::Float64& state_msg); + void printParameters(); + void reconfigureCallback(pid::PidConfig& config, uint32_t level); + void setpointCallback(const std_msgs::Float64& setpoint_msg); + bool validateParameters(); + + // Primary PID controller input & output variables + double plant_state_; // current output of plant + double control_effort_ = 0; // output of pid controller + double setpoint_ = 0; // desired output of plant + bool pid_enabled_ = true; // PID is enabled to run + bool new_state_or_setpt_ = false; // Indicate that fresh calculations need to be run + + ros::Time prev_time_; + ros::Duration delta_t_; + bool first_reconfig_ = true; + + double error_integral_ = 0; + double proportional_ = 0; // proportional term of output + double integral_ = 0; // integral term of output + double derivative_ = 0; // derivative term of output + + // PID gains + double Kp_ = 0, Ki_ = 0, Kd_ = 0; + + // Parameters for error calc. with disconinuous input + bool angle_error_ = false; + double angle_wrap_ = 2.0 * 3.14159; + + // Cutoff frequency for the derivative calculation in Hz. + // Negative -> Has not been set by the user yet, so use a default. + double cutoff_frequency_ = -1; + + // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency + // at + // 1/4 of the sample rate. + double c_ = 1.; + + // Used to check for tan(0)==>NaN in the filter calculation + double tan_filt_ = 1.; + + // Upper and lower saturation limits + double upper_limit_ = 1000, lower_limit_ = -1000; + + // Anti-windup term. Limits the absolute value of the integral term. + double windup_limit_ = 1000; + + // Initialize filter data with zeros + std::vector error_, filtered_error_, error_deriv_, filtered_error_deriv_; + + // Topic and node names and message objects + ros::Publisher control_effort_pub_; + std::string topic_from_controller_, topic_from_plant_, setpoint_topic_, pid_enable_topic_; + + std_msgs::Float64 control_msg_, state_msg_; + + // Diagnostic objects + double min_loop_frequency_ = 1, max_loop_frequency_ = 1000; + int measurements_received_ = 0; +}; +} // end pid namespace + +#endif diff --git a/src/pid/include/pid/pid.h b/src/pid/include/pid/pid.h new file mode 100644 index 0000000..720880f --- /dev/null +++ b/src/pid/include/pid/pid.h @@ -0,0 +1,133 @@ +/***************************************************************************/ /** + * \file controller.h + * + * \brief Simple PID controller with dynamic reconfigure + * \author Andy Zelenak + * \date March 8, 2015 + * + * \section license License (BSD-3) + * Copyright (c) 2015, Andy Zelenak\n + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#ifndef PID_H +#define PID_H + +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pid_ns +{ +class PidObject +{ +public: + PidObject(); + + // Primary output variable + double control_effort_ = 0; // output of pid controller + +private: + void doCalcs(); + void getParams(double in, double& value, double& scale); + void pidEnableCallback(const std_msgs::Bool& pid_enable_msg); + void plantStateCallback(const std_msgs::Float64& state_msg); + void printParameters(); + void reconfigureCallback(pid::PidConfig& config, uint32_t level); + void setpointCallback(const std_msgs::Float64& setpoint_msg); + bool validateParameters(); + + // Primary PID controller input variables + double plant_state_; // current output of plant + bool pid_enabled_ = true; // PID is enabled to run + bool new_state_or_setpt_ = false; // Indicate that fresh calculations need to be run + double setpoint_ = 0; // desired output of plant + + ros::Time prev_time_; + ros::Time last_setpoint_msg_time_; + ros::Duration delta_t_; + bool first_reconfig_ = true; + + double error_integral_ = 0; + double proportional_ = 0; // proportional term of output + double integral_ = 0; // integral term of output + double derivative_ = 0; // derivative term of output + + // PID gains + double Kp_ = 0, Ki_ = 0, Kd_ = 0; + + // Parameters for error calc. with disconinuous input + bool angle_error_ = false; + double angle_wrap_ = 2.0 * 3.14159; + + // Cutoff frequency for the derivative calculation in Hz. + // Negative -> Has not been set by the user yet, so use a default. + double cutoff_frequency_ = -1; + + // Setpoint timeout parameter to determine how long to keep publishing + // control_effort messages after last setpoint message + // -1 indicates publish indefinately, and positive number sets the timeout + double setpoint_timeout_ = -1; + + // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency + // at + // 1/4 of the sample rate. + double c_ = 1.; + + // Used to check for tan(0)==>NaN in the filter calculation + double tan_filt_ = 1.; + + // Upper and lower saturation limits + double upper_limit_ = 1000, lower_limit_ = -1000; + + // Anti-windup term. Limits the absolute value of the integral term. + double windup_limit_ = 1000; + + // Initialize filter data with zeros + std::vector error_, filtered_error_, error_deriv_, filtered_error_deriv_; + + // Topic and node names and message objects + ros::Publisher control_effort_pub_; + ros::Publisher pid_debug_pub_; + + std::string topic_from_controller_, topic_from_plant_, setpoint_topic_, pid_enable_topic_; + std::string pid_debug_pub_name_; + std_msgs::Float64 control_msg_, state_msg_; + + // Diagnostic objects + double min_loop_frequency_ = 1, max_loop_frequency_ = 1000; + int measurements_received_ = 0; +}; +} // end pid namespace + +#endif diff --git a/src/pid/launch/differential_drive_sim.launch b/src/pid/launch/differential_drive_sim.launch new file mode 100644 index 0000000..f14e2de --- /dev/null +++ b/src/pid/launch/differential_drive_sim.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/pid/launch/run_camera_rotation.launch b/src/pid/launch/run_camera_rotation.launch new file mode 100644 index 0000000..1d1b497 --- /dev/null +++ b/src/pid/launch/run_camera_rotation.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/pid/launch/servo_sim.launch.backup b/src/pid/launch/servo_sim.launch.backup new file mode 100644 index 0000000..1c163b4 --- /dev/null +++ b/src/pid/launch/servo_sim.launch.backup @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/pid/launch/sim_time.launch b/src/pid/launch/sim_time.launch new file mode 100644 index 0000000..8c08b69 --- /dev/null +++ b/src/pid/launch/sim_time.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/pid/license.txt b/src/pid/license.txt new file mode 100644 index 0000000..e5b20d9 --- /dev/null +++ b/src/pid/license.txt @@ -0,0 +1,12 @@ +Copyright (c) 2015, Andy Zelenak +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/pid/package.xml b/src/pid/package.xml new file mode 100644 index 0000000..81ac990 --- /dev/null +++ b/src/pid/package.xml @@ -0,0 +1,25 @@ + + + pid + 0.0.28 + Launch a PID control node. + + Andy Zelenak + + BSD + + http://wiki.ros.org/pid + + Andy Zelenak + Paul Bouchier + + + catkin + + + message_runtime + roscpp + std_msgs + dynamic_reconfigure + + diff --git a/src/pid/src/autotune.cpp b/src/pid/src/autotune.cpp new file mode 100644 index 0000000..9d743e5 --- /dev/null +++ b/src/pid/src/autotune.cpp @@ -0,0 +1,254 @@ +/***************************************************************************/ /** + * \file autotune.cpp + * + * \brief Autotune a PID controller with the Ziegler Nichols method + * \author Andy Zelenak + * \date October 25, 2016 + * + * \section license License (BSD-3) + * Copyright (c) 2016, Andy Zelenak\n + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +// Use the Ziegler Nichols Method to calculate reasonable PID gains. +// The ZN Method is based on setting Ki & Kd to zero, then cranking up Kp until +// oscillations are observed. +// This node varies Kp through a range until oscillations are just barely +// observed, +// then calculates the other parameters automatically. +// See https://en.wikipedia.org/wiki/PID_controller + +#include +#include +#include +#include + +// Use dynamic_reconfigure to adjust Kp, Ki, and Kd +#include +#include +#include + +void setKiKdToZero(); +void setKp(double Kp); +void setpointCallback(const std_msgs::Float64& setpoint_msg); +void stateCallback(const std_msgs::Float64& state_msg); +void setFinalParams(); + +namespace autotune +{ +double Ku = 0.; +double Tu = 0.; +double setpoint = 0.; +double state = 0.; +std::string ns = "/left_wheel_pid/"; +int oscillation_count = 0; +int num_loops = 100; // Will look for oscillations for num_loops*loopRate +int initial_error = 0; +double Kp_ZN = 0.; +double Ki_ZN = 0.; +double Kd_ZN = 0.; +bool found_Ku = false; +std::vector oscillation_times(3); // Used to calculate Tu, the oscillation period +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "autotune_node"); + ros::NodeHandle autotune_node; + ros::start(); + ros::Subscriber setpoint_sub = autotune_node.subscribe("/setpoint", 1, setpointCallback); + ros::Subscriber state_sub = autotune_node.subscribe("/state", 1, stateCallback); + ros::Rate loopRate(50); + + // Set Ki and Kd to zero for the ZN method with dynamic_reconfigure + setKiKdToZero(); + + // Define how rapidly the value of Kp is varied, and the max/min values to try + double Kp_max = 10.; + double Kp_min = 0.5; + double Kp_step = 1.0; + + for (double Kp = Kp_min; Kp <= Kp_max; Kp += Kp_step) + { + ////////////////////// + // Get the error sign. + ////////////////////// + // Need to wait for new setpoint/state msgs + ros::topic::waitForMessage("setpoint"); + ros::topic::waitForMessage("state"); + + // Try a new Kp. + setKp(Kp); + ROS_INFO_STREAM("Trying Kp = " << Kp); // Blank line on terminal + autotune::oscillation_count = 0; // Reset to look for oscillations again + + for (int i = 0; i < autotune::num_loops; i++) // Collect data for loopRate*num_loops seconds + { + ros::spinOnce(); + loopRate.sleep(); + if (i == 0) // Get the sign of the initial error + { + autotune::initial_error = (autotune::setpoint - autotune::state); + } + + // Did the system oscillate about the setpoint? If so, Kp~Ku. + // Oscillation => the sign of the error changes + // The first oscillation is caused by the change in setpoint. Ignore it. + // Look for 2 oscillations. + // Get a fresh state message + ros::topic::waitForMessage("state"); + double new_error = (autotune::setpoint - autotune::state); // Sign of the error + // ROS_INFO_STREAM("New error: "<< new_error); + if (std::signbit(autotune::initial_error) != std::signbit(new_error)) + { + autotune::oscillation_times.at(autotune::oscillation_count) = + loopRate.expectedCycleTime().toSec() * i; // Record the time to calculate a period, Tu + autotune::oscillation_count++; + ROS_INFO_STREAM("Oscillation occurred. Oscillation count: " << autotune::oscillation_count); + autotune::initial_error = new_error; // Reset to look for another oscillation + + // If the system is definitely oscillating about the setpoint + if (autotune::oscillation_count > 2) + { + // Now calculate the period of oscillation (Tu) + autotune::Tu = autotune::oscillation_times.at(2) - autotune::oscillation_times.at(0); + ROS_INFO_STREAM("Tu (oscillation period): " << autotune::Tu); + // ROS_INFO_STREAM( "2*sampling period: " << + // 2.*loopRate.expectedCycleTime().toSec() ); + + // We're looking for more than just the briefest dip across the + // setpoint and back. + // Want to see significant oscillation + if (autotune::Tu > 3. * loopRate.expectedCycleTime().toSec()) + { + autotune::Ku = Kp; + + // Now calculate the other parameters with ZN method + autotune::Kp_ZN = 0.6 * autotune::Ku; + autotune::Ki_ZN = 1.2 * autotune::Ku / autotune::Tu; + autotune::Kd_ZN = 3. * autotune::Ku * autotune::Tu / 40.; + + autotune::found_Ku = true; + goto DONE; + } + else + break; // Try the next Kp + } + } + } + } +DONE: + + if (autotune::found_Ku == true) + { + setFinalParams(); + } + else + ROS_INFO_STREAM("Did not see any oscillations for this range of Kp. Adjust " + "Kp_max and Kp_min to broaden the search."); + + ros::shutdown(); + return 0; +} + +// Set Ki and Kd to zero with dynamic_reconfigure +void setKiKdToZero() +{ + dynamic_reconfigure::ReconfigureRequest srv_req; + dynamic_reconfigure::ReconfigureResponse srv_resp; + dynamic_reconfigure::DoubleParameter double_param; + dynamic_reconfigure::Config config; + double_param.name = "Ki"; + double_param.value = 0.0; + config.doubles.push_back(double_param); + double_param.name = "Kd"; + double_param.value = 0.0; + config.doubles.push_back(double_param); + srv_req.config = config; + ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp); +} + +// Set Kp with dynamic_reconfigure +void setKp(double Kp) +{ + dynamic_reconfigure::ReconfigureRequest srv_req; + dynamic_reconfigure::ReconfigureResponse srv_resp; + dynamic_reconfigure::DoubleParameter double_param; + dynamic_reconfigure::Config config; + + // A blank service call to get the current parameters into srv_resp + ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp); + + double_param.name = "Kp"; + double_param.value = Kp / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI + config.doubles.push_back(double_param); + srv_req.config = config; + ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp); +} + +void setpointCallback(const std_msgs::Float64& setpoint_msg) +{ + autotune::setpoint = setpoint_msg.data; +} + +void stateCallback(const std_msgs::Float64& state_msg) +{ + autotune::state = state_msg.data; +} + +// Print out and set the final parameters as calculated by the autotuner +void setFinalParams() +{ + ROS_INFO_STREAM(" "); + ROS_INFO_STREAM("The suggested parameters are: "); + ROS_INFO_STREAM("Kp " << autotune::Kp_ZN); + ROS_INFO_STREAM("Ki " << autotune::Ki_ZN); + ROS_INFO_STREAM("Kd " << autotune::Kd_ZN); + + // Set the ZN parameters with dynamic_reconfigure + dynamic_reconfigure::ReconfigureRequest srv_req; + dynamic_reconfigure::ReconfigureResponse srv_resp; + dynamic_reconfigure::DoubleParameter double_param; + dynamic_reconfigure::Config config; + + // A blank service call to get the current parameters into srv_resp + ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp); + + double_param.name = "Kp"; + double_param.value = autotune::Kp_ZN / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI + config.doubles.push_back(double_param); + + double_param.name = "Ki"; + double_param.value = autotune::Ki_ZN / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI + config.doubles.push_back(double_param); + + double_param.name = "Kd"; + double_param.value = autotune::Kd_ZN / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI + config.doubles.push_back(double_param); + + srv_req.config = config; + ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp); +} \ No newline at end of file diff --git a/src/pid/src/controller.cpp b/src/pid/src/controller.cpp new file mode 100644 index 0000000..e547b3b --- /dev/null +++ b/src/pid/src/controller.cpp @@ -0,0 +1,49 @@ +/***************************************************************************/ /** + * \file controller.cpp + * + * \brief Simple PID controller with dynamic reconfigure + * \author Andy Zelenak + * \date March 8, 2015 + * + * \section license License (BSD-3) + * Copyright (c) 2015, Andy Zelenak\n + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +// Subscribe to a topic about the state of a dynamic system and calculate +// feedback to +// stabilize it. + +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "controller"); + + pid_ns::PidObject my_pid; + + return 0; +} \ No newline at end of file diff --git a/src/pid/src/pid.cpp b/src/pid/src/pid.cpp new file mode 100644 index 0000000..1b0eebf --- /dev/null +++ b/src/pid/src/pid.cpp @@ -0,0 +1,323 @@ + +#include + +using namespace pid_ns; + +PidObject::PidObject() : error_(3, 0), filtered_error_(3, 0), error_deriv_(3, 0), filtered_error_deriv_(3, 0) +{ + ros::NodeHandle node; + ros::NodeHandle node_priv("~"); + + while (ros::ok() && ros::Time(0) == ros::Time::now()) + { + ROS_INFO("controller spinning, waiting for time to become non-zero"); + sleep(1); + } + + // Get params if specified in launch file or as params on command-line, set + // defaults + node_priv.param("Kp", Kp_, 1.0); + node_priv.param("Ki", Ki_, 0.0); + node_priv.param("Kd", Kd_, 0.0); + node_priv.param("upper_limit", upper_limit_, 1000.0); + node_priv.param("lower_limit", lower_limit_, -1000.0); + node_priv.param("windup_limit", windup_limit_, 1000.0); + node_priv.param("cutoff_frequency", cutoff_frequency_, -1.0); + node_priv.param("topic_from_controller", topic_from_controller_, "control_effort"); + node_priv.param("topic_from_plant", topic_from_plant_, "state"); + node_priv.param("setpoint_topic", setpoint_topic_, "setpoint"); + node_priv.param("pid_enable_topic", pid_enable_topic_, "pid_enable"); + node_priv.param("max_loop_frequency", max_loop_frequency_, 1.0); + node_priv.param("min_loop_frequency", min_loop_frequency_, 1000.0); + node_priv.param("pid_debug_topic", pid_debug_pub_name_, "pid_debug"); + node_priv.param("setpoint_timeout", setpoint_timeout_, -1.0); + ROS_ASSERT_MSG(setpoint_timeout_ ==-1 || setpoint_timeout_ > 0, + "setpoint_timeout set to %.2f but needs to -1 or >0", setpoint_timeout_); + + // Two parameters to allow for error calculation with discontinous value + node_priv.param("angle_error", angle_error_, false); + node_priv.param("angle_wrap", angle_wrap_, 2.0 * 3.14159); + + // Update params if specified as command-line options, & print settings + printParameters(); + if (not validateParameters()) + std::cout << "Error: invalid parameter\n"; + + // instantiate publishers & subscribers + control_effort_pub_ = node.advertise(topic_from_controller_, 1); + pid_debug_pub_ = node.advertise(pid_debug_pub_name_, 1); + + ros::Subscriber plant_sub_ = node.subscribe(topic_from_plant_, 1, &PidObject::plantStateCallback, this); + ros::Subscriber setpoint_sub_ = node.subscribe(setpoint_topic_, 1, &PidObject::setpointCallback, this); + ros::Subscriber pid_enabled_sub_ = node.subscribe(pid_enable_topic_, 1, &PidObject::pidEnableCallback, this); + + if (!plant_sub_ || !setpoint_sub_ || !pid_enabled_sub_) + { + ROS_ERROR_STREAM("Initialization of a subscriber failed. Exiting."); + ros::shutdown(); + exit(EXIT_FAILURE); + } + + // dynamic reconfiguration + dynamic_reconfigure::Server config_server; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&PidObject::reconfigureCallback, this, _1, _2); + config_server.setCallback(f); + + // Wait for first messages + while( ros::ok() && !ros::topic::waitForMessage(setpoint_topic_, ros::Duration(10.))) + ROS_WARN_STREAM("Waiting for first setpoint message."); + + while( ros::ok() && !ros::topic::waitForMessage(topic_from_plant_, ros::Duration(10.))) + ROS_WARN_STREAM("Waiting for first state message from the plant."); + + // Respond to inputs until shut down + while (ros::ok()) + { + doCalcs(); + ros::spinOnce(); + + // Add a small sleep to avoid 100% CPU usage + ros::Duration(0.001).sleep(); + } +}; + +void PidObject::setpointCallback(const std_msgs::Float64& setpoint_msg) +{ + setpoint_ = setpoint_msg.data; + last_setpoint_msg_time_ = ros::Time::now(); + new_state_or_setpt_ = true; +} + +void PidObject::plantStateCallback(const std_msgs::Float64& state_msg) +{ + plant_state_ = state_msg.data; + + new_state_or_setpt_ = true; +} + +void PidObject::pidEnableCallback(const std_msgs::Bool& pid_enable_msg) +{ + pid_enabled_ = pid_enable_msg.data; +} + +void PidObject::getParams(double in, double& value, double& scale) +{ + int digits = 0; + value = in; + while (ros::ok() && ((fabs(value) > 1.0 || fabs(value) < 0.1) && (digits < 2 && digits > -1))) + { + if (fabs(value) > 1.0) + { + value /= 10.0; + digits++; + } + else + { + value *= 10.0; + digits--; + } + } + if (value > 1.0) + value = 1.0; + if (value < -1.0) + value = -1.0; + + scale = pow(10.0, digits); +} + +bool PidObject::validateParameters() +{ + if (lower_limit_ > upper_limit_) + { + ROS_ERROR("The lower saturation limit cannot be greater than the upper " + "saturation limit."); + return (false); + } + + return true; +} + +void PidObject::printParameters() +{ + std::cout << std::endl << "PID PARAMETERS" << std::endl << "-----------------------------------------" << std::endl; + std::cout << "Kp: " << Kp_ << ", Ki: " << Ki_ << ", Kd: " << Kd_ << std::endl; + if (cutoff_frequency_ == -1) // If the cutoff frequency was not specified by the user + std::cout << "LPF cutoff frequency: 1/4 of sampling rate" << std::endl; + else + std::cout << "LPF cutoff frequency: " << cutoff_frequency_ << std::endl; + std::cout << "pid node name: " << ros::this_node::getName() << std::endl; + std::cout << "Name of topic from controller: " << topic_from_controller_ << std::endl; + std::cout << "Name of topic from the plant: " << topic_from_plant_ << std::endl; + std::cout << "Name of setpoint topic: " << setpoint_topic_ << std::endl; + std::cout << "Integral-windup limit: " << windup_limit_ << std::endl; + std::cout << "Saturation limits: " << upper_limit_ << "/" << lower_limit_ << std::endl; + std::cout << "-----------------------------------------" << std::endl; + + return; +} + +void PidObject::reconfigureCallback(pid::PidConfig& config, uint32_t level) +{ + if (first_reconfig_) + { + getParams(Kp_, config.Kp, config.Kp_scale); + getParams(Ki_, config.Ki, config.Ki_scale); + getParams(Kd_, config.Kd, config.Kd_scale); + first_reconfig_ = false; + return; // Ignore the first call to reconfigure which happens at startup + } + + Kp_ = config.Kp * config.Kp_scale; + Ki_ = config.Ki * config.Ki_scale; + Kd_ = config.Kd * config.Kd_scale; + ROS_INFO("Pid reconfigure request: Kp: %f, Ki: %f, Kd: %f", Kp_, Ki_, Kd_); +} + +void PidObject::doCalcs() +{ + // Do fresh calcs if knowledge of the system has changed. + if (new_state_or_setpt_) + { + if (!((Kp_ <= 0. && Ki_ <= 0. && Kd_ <= 0.) || + (Kp_ >= 0. && Ki_ >= 0. && Kd_ >= 0.))) // All 3 gains should have the same sign + ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for " + "stability."); + + error_.at(2) = error_.at(1); + error_.at(1) = error_.at(0); + error_.at(0) = setpoint_ - plant_state_; // Current error goes to slot 0 + + // If the angle_error param is true, then address discontinuity in error + // calc. + // For example, this maintains an angular error between -180:180. + if (angle_error_) + { + while (error_.at(0) < -1.0 * angle_wrap_ / 2.0) + { + error_.at(0) += angle_wrap_; + + // The proportional error will flip sign, but the integral error + // won't and the filtered derivative will be poorly defined. So, + // reset them. + error_deriv_.at(2) = 0.; + error_deriv_.at(1) = 0.; + error_deriv_.at(0) = 0.; + error_integral_ = 0.; + } + while (error_.at(0) > angle_wrap_ / 2.0) + { + error_.at(0) -= angle_wrap_; + + // The proportional error will flip sign, but the integral error + // won't and the filtered derivative will be poorly defined. So, + // reset them. + error_deriv_.at(2) = 0.; + error_deriv_.at(1) = 0.; + error_deriv_.at(0) = 0.; + error_integral_ = 0.; + } + } + + // calculate delta_t + if (!prev_time_.isZero()) // Not first time through the program + { + delta_t_ = ros::Time::now() - prev_time_; + prev_time_ = ros::Time::now(); + if (0 == delta_t_.toSec()) + { + ROS_ERROR("delta_t is 0, skipping this loop. Possible overloaded cpu " + "at time: %f", + ros::Time::now().toSec()); + return; + } + } + else + { + ROS_INFO("prev_time is 0, doing nothing"); + prev_time_ = ros::Time::now(); + return; + } + + // integrate the error + error_integral_ += error_.at(0) * delta_t_.toSec(); + + // Apply windup limit to limit the size of the integral term + if (error_integral_ > fabsf(windup_limit_)) + error_integral_ = fabsf(windup_limit_); + + if (error_integral_ < -fabsf(windup_limit_)) + error_integral_ = -fabsf(windup_limit_); + + // My filter reference was Julius O. Smith III, Intro. to Digital Filters + // With Audio Applications. + // See https://ccrma.stanford.edu/~jos/filters/Example_Second_Order_Butterworth_Lowpass.html + if (cutoff_frequency_ != -1) + { + // Check if tan(_) is really small, could cause c = NaN + tan_filt_ = tan((cutoff_frequency_ * 6.2832) * delta_t_.toSec() / 2); + + // Avoid tan(0) ==> NaN + if ((tan_filt_ <= 0.) && (tan_filt_ > -0.01)) + tan_filt_ = -0.01; + if ((tan_filt_ >= 0.) && (tan_filt_ < 0.01)) + tan_filt_ = 0.01; + + c_ = 1 / tan_filt_; + } + + filtered_error_.at(2) = filtered_error_.at(1); + filtered_error_.at(1) = filtered_error_.at(0); + filtered_error_.at(0) = (1 / (1 + c_ * c_ + 1.414 * c_)) * (error_.at(2) + 2 * error_.at(1) + error_.at(0) - + (c_ * c_ - 1.414 * c_ + 1) * filtered_error_.at(2) - + (-2 * c_ * c_ + 2) * filtered_error_.at(1)); + + // Take derivative of error + // First the raw, unfiltered data: + error_deriv_.at(2) = error_deriv_.at(1); + error_deriv_.at(1) = error_deriv_.at(0); + error_deriv_.at(0) = (error_.at(0) - error_.at(1)) / delta_t_.toSec(); + + filtered_error_deriv_.at(2) = filtered_error_deriv_.at(1); + filtered_error_deriv_.at(1) = filtered_error_deriv_.at(0); + + filtered_error_deriv_.at(0) = + (1 / (1 + c_ * c_ + 1.414 * c_)) * + (error_deriv_.at(2) + 2 * error_deriv_.at(1) + error_deriv_.at(0) - + (c_ * c_ - 1.414 * c_ + 1) * filtered_error_deriv_.at(2) - (-2 * c_ * c_ + 2) * filtered_error_deriv_.at(1)); + + // calculate the control effort + proportional_ = Kp_ * filtered_error_.at(0); + integral_ = Ki_ * error_integral_; + derivative_ = Kd_ * filtered_error_deriv_.at(0); + control_effort_ = proportional_ + integral_ + derivative_; + + // Apply saturation limits + if (control_effort_ > upper_limit_) + control_effort_ = upper_limit_; + else if (control_effort_ < lower_limit_) + control_effort_ = lower_limit_; + + // Publish the stabilizing control effort if the controller is enabled + if (pid_enabled_ && (setpoint_timeout_ == -1 || + (ros::Time::now() - last_setpoint_msg_time_).toSec() <= setpoint_timeout_)) + { + control_msg_.data = control_effort_; + control_effort_pub_.publish(control_msg_); + // Publish topic with + std::vector pid_debug_vect { plant_state_, control_effort_, proportional_, integral_, derivative_}; + std_msgs::Float64MultiArray pidDebugMsg; + pidDebugMsg.data = pid_debug_vect; + pid_debug_pub_.publish(pidDebugMsg); + } + else if (setpoint_timeout_ > 0 && (ros::Time::now() - last_setpoint_msg_time_).toSec() > setpoint_timeout_) + { + ROS_WARN_ONCE("Setpoint message timed out, will stop publising control_effort_messages"); + error_integral_ = 0.0; + } + else + error_integral_ = 0.0; + } + + new_state_or_setpt_ = false; +} diff --git a/src/pid/src/plant_sim.cpp b/src/pid/src/plant_sim.cpp new file mode 100644 index 0000000..f43e843 --- /dev/null +++ b/src/pid/src/plant_sim.cpp @@ -0,0 +1,173 @@ +/***************************************************************************/ /** + * \file plant_sim.h + * + * \brief First or second order plant simulator + * \author Andy Zelenak + * \author Paul Bouchier + * \date March 8, 2015 + * + * \section license License (BSD-3) + * Copyright (c) 2015, Paul Bouchier\n + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +// This file simulates a 1st or 2nd-order dynamic system, publishes its state, +// and subscribes to a 'control_effort' topic. The control effort is used +// to stabilize the servo. + +#include +#include + +namespace plant_sim +{ +// Global so it can be passed from the callback fxn to main +static double control_effort = 0.0; +static bool reverse_acting = false; +} +using namespace plant_sim; + +// Callback when something is published on 'control_effort' +void controlEffortCallback(const std_msgs::Float64& control_effort_input) +{ + // the stabilizing control effort + if (reverse_acting) + { + control_effort = -control_effort_input.data; + } + else + { + control_effort = control_effort_input.data; + } +} + +int main(int argc, char** argv) +{ + int plant_order = 1; + double temp = 4.7; // initial condition for first-order plant + double displacement = 3.3; // initial condition for second-order plant + + ros::init(argc, argv, "plant"); + ros::NodeHandle sim_node; + + while (ros::ok() && ros::Time(0) == ros::Time::now()) + { + ROS_INFO("Plant_sim spinning waiting for time to become non-zero"); + sleep(1); + } + + ros::NodeHandle node_priv("~"); + node_priv.param("plant_order", plant_order, 1); + node_priv.param("reverse_acting", reverse_acting, false); + + if (plant_order == 1) + { + ROS_INFO("Starting simulation of a first-order plant."); + } + else if (plant_order == 2) + { + ROS_INFO("Starting simulation of a second-order plant."); + } + else + { + ROS_ERROR("Error: Invalid plant type parameter, must be 1 or 2: %s", argv[1]); + return -1; + } + + // Advertise a plant state msg + std_msgs::Float64 plant_state; + ros::Publisher servo_state_pub = sim_node.advertise("state", 1); + + // Subscribe to "control_effort" topic to get a controller_msg.msg + ros::Subscriber sub = sim_node.subscribe("control_effort", 1, controlEffortCallback); + + int loop_counter = 0; + double delta_t = 0.01; + ros::Rate loop_rate(1 / delta_t); // Control rate in Hz + + // Initialize 1st-order (e.g temp controller) process variables + double temp_rate = 0; // rate of temp change + + // Initialize 2nd-order (e.g. servo-motor with load) process variables + double speed = 0; // meters/sec + double acceleration = 0; // meters/sec^2 + double mass = 0.1; // in kg + double friction = 1.0; // a decelerating force factor + double stiction = 1; // control_effort must exceed this before stationary servo moves + double Kv = 1; // motor constant: force (newtons) / volt + double Kbackemf = 0; // Volts of back-emf per meter/sec of speed + double decel_force; // decelerating force + + while (ros::ok()) + { + ros::spinOnce(); + + switch (plant_order) + { + case 1: // First order plant + temp_rate = (0.1 * temp) + control_effort; + temp = temp + temp_rate * delta_t; + + plant_state.data = temp; + break; + + case 2: // Second order plant + if (fabs(speed) < 0.001) + { + // if nearly stopped, stop it & require overcoming stiction to restart + speed = 0; + if (fabs(control_effort) < stiction) + { + control_effort = 0; + } + } + + // Update the servo. + // control_effort: the voltage applied to the servo. Output from PID + // controller. It is + // opposed by back emf (expressed as speed) to produce a net force. + // Range: -1 to +1 + // displacement: the actual value of the servo output position. Input to + // PID controller + + decel_force = -(speed * friction); // can be +ve or -ve. Linear with speed + acceleration = ((Kv * (control_effort - (Kbackemf * speed)) + decel_force) / mass); // a = F/m + speed = speed + (acceleration * delta_t); + displacement = displacement + speed * delta_t; + + plant_state.data = displacement; + break; + + default: + ROS_ERROR("Invalid plant_order: %d", plant_order); + return (-1); + } + + servo_state_pub.publish(plant_state); + loop_rate.sleep(); + } + + return 0; +} diff --git a/src/pid/src/setpoint_node.cpp b/src/pid/src/setpoint_node.cpp new file mode 100644 index 0000000..f5c0c16 --- /dev/null +++ b/src/pid/src/setpoint_node.cpp @@ -0,0 +1,67 @@ +/***************************************************************************/ /** + * \file setpoint_node.cpp + * + * \brief Node that publishes time-varying setpoint values + * \author Paul Bouchier + * \date January 9, 2016 + * + * \section license License (BSD-3) + * Copyright (c) 2016, Paul Bouchier + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include "ros/ros.h" +#include "std_msgs/Float64.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "setpoint_node"); + ROS_INFO("Starting setpoint publisher"); + ros::NodeHandle setpoint_node; + + while (ros::ok() && ros::Time(0) == ros::Time::now()) + { + ROS_INFO("Setpoint_node spinning, waiting for time to become non-zero"); + sleep(1); + } + + std_msgs::Float64 setpoint; + setpoint.data = 1.0; + ros::Publisher setpoint_pub = setpoint_node.advertise("setpoint", 1); + + ros::Rate loop_rate(0.2); // change setpoint every 5 seconds + + while (ros::ok()) + { + ros::spinOnce(); + + setpoint_pub.publish(setpoint); // publish twice so graph gets it as a step + setpoint.data = 0 - setpoint.data; + setpoint_pub.publish(setpoint); + + loop_rate.sleep(); + } +} diff --git a/src/pid/src/sim_time.cpp b/src/pid/src/sim_time.cpp new file mode 100644 index 0000000..2cd6576 --- /dev/null +++ b/src/pid/src/sim_time.cpp @@ -0,0 +1,94 @@ +/***************************************************************************/ /** + * \file sim_time.cpp + * + * \brief Node that publishes simulated time to the /clock topic + * + * \author Paul Bouchier + * \date January 27, 2016 + * + * \section license License (BSD-3) + * Copyright (c) 2016, Paul Bouchier + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include "ros/ros.h" +#include "rosgraph_msgs/Clock.h" + +#include + +#define SIM_TIME_INCREMENT_US 10000 + +/* + * This node publishes increments of 1ms in time to the /clock topic. It does so + * at a rate determined by sim_speedup (simulation speedup factor), which should + * be passed + * in as a private parameter. + */ + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "sim_time_source"); + ros::NodeHandle sim_time_node; + + // support integral multiples of wallclock time for simulation speedup + int sim_speedup; // integral factor by which to speed up simulation + ros::NodeHandle node_priv("~"); + node_priv.param("sim_speedup", sim_speedup, 1); + + // get the current time & populate sim_time with it + struct timeval start; + int rv = gettimeofday(&start, NULL); + usleep(1000); + struct timeval now; + rv = gettimeofday(&now, NULL); + if (0 != rv) + { + ROS_ERROR("Invalid return from gettimeofday: %d", rv); + return -1; + } + + rosgraph_msgs::Clock sim_time; + sim_time.clock.sec = now.tv_sec - start.tv_sec; + sim_time.clock.nsec = now.tv_usec * 1000; + ros::Publisher sim_time_pub = sim_time_node.advertise("clock", 1); + + ROS_INFO("Starting simulation time publisher at time: %d.%d", sim_time.clock.sec, sim_time.clock.nsec); + + while (ros::ok()) + { + sim_time_pub.publish(sim_time); + + sim_time.clock.nsec = sim_time.clock.nsec + SIM_TIME_INCREMENT_US * 1000; + while (sim_time.clock.nsec > 1000000000) + { + sim_time.clock.nsec -= 1000000000; + ++sim_time.clock.sec; + } + + usleep(SIM_TIME_INCREMENT_US / sim_speedup); + ros::spinOnce(); + } +} \ No newline at end of file