Giter Club home page Giter Club logo

ros2_pid_library's Introduction

Hi 👋 My name is Antonio Mauro Galiano

C++ Software Engineer and ROS expert

Full stack Robotics Software Engineer, I've amassed about 10 years' experience in the world of ground mobile robotics developing packages for the Robot Operating System ROS. My daily food is research and develop.

  • 🌍  I'm based in Bologna
  • 🧠  I'm learning ROS2 and Python3
  • 🤝  I'm open to collaborating on ROS and ROS2 projects
  • ⚡  I love robotics (of course), videogames, wellness, football, wrestling and sports in general, animals especially cats

Skills

C C++ Python PostgreSQL

Socials

Badges

My GitHub Stats

GitHub Commits Graph

Top Repositories






ros2_pid_library's People

Contributors

dottantgal avatar

Stargazers

avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar

Watchers

avatar avatar avatar avatar avatar avatar

Forkers

szepilot capitaneanu collinnhays

ros2_pid_library's Issues

Implementation and Optimization of Separate PID Controls for Dual-Wheel System

I am currently working on a project involving a dual-wheel system where each wheel requires independent PID control. The goal is to achieve precise and synchronized control of both wheels, ensuring stable and efficient operation of the system.

Challenges faced:

  1. Tuning PID parameters for each wheel separately to accommodate different load and environmental conditions.
  2. Handling situations where the delta time becomes zero, potentially affecting the derivative calculation in the PID loop.
  3. Ensuring that the control values for each wheel converge properly, especially under varying set points and external influences.

I am seeking advice and suggestions on the following:

  • Strategies for effective tuning of PID parameters for each wheel.
  • Best practices for handling zero delta time in PID calculations.
  • Ideas for ensuring robust and responsive control of each wheel, particularly in scenarios with rapidly changing set points or external disturbances.

Any insights, experiences, or references to similar projects would be greatly appreciated. Additionally, if there are suggestions for code optimization or error handling in such a dual-wheel PID control system, please share them.

Thank you in advance for your contributions to this discussion.

Related Code Snippets:
pid_library.h file:

#ifndef PID_LIBRARY_H
#define PID_LIBRARY_H

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/duration.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/float32.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include <chrono>

using namespace std::chrono_literals;

class PidLibrary : public rclcpp::Node
{
public:
 PidLibrary();
 ~PidLibrary();
 void UsePidLibrary();

protected:
 // Publishers for each wheel
 rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr controlValuePubL_, controlValuePubR_;

 // Subscriptions for each wheel
 rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr actualStateSubL_, actualStateSubR_;
 rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr setPointSubL_, setPointSubR_;

 // Time variables
 rclcpp::Time currentTime_;
 rclcpp::Time previousTime_;
 rclcpp::Duration deltaTime_ = rclcpp::Duration(0ns);

 // PID parameters and variables for each wheel
 float kp_left_, ki_left_, kd_left_;
 float kp_right_, ki_right_, kd_right_;
 float actualStateValueL_, actualStateValueR_;
 float setPointValueL_, setPointValueR_;
 float errorIntegralLeft_, errorIntegralRight_;
 float controlValueL_, controlValueR_;
 float oldActualStateValueL_, oldActualStateValueR_;
 float oldErrorL_, oldErrorR_;
 int outMinLeft_, outMaxLeft_;
 int outMinRight_, outMaxRight_;

 // Shared PID settings
 int sampleTime_;
 bool useSampleTime_;
 bool derivativeOnMeasurement_;
 bool removeKiBump_;
 bool resetWindup_;
 bool pidEnabled_;
 bool makeReverse_;
 bool isTheSystemChangedR_, isTheSystemChangedL_ ;
 // Topic names
 std::string controlValueTopicL_, controlValueTopicR_;
 std::string actualStateTopicL_, actualStateTopicR_;
 std::string setPointTopicL_, setPointTopicR_;

 // Timer and parameter callback handle
 rclcpp::TimerBase::SharedPtr timer_;
 OnSetParametersCallbackHandle::SharedPtr paramCallbackHandle_;

 // Callback methods for each wheel
 void ActualStateCallbackLeft(const std_msgs::msg::Float32::SharedPtr actualStateMsg);
 void ActualStateCallbackRight(const std_msgs::msg::Float32::SharedPtr actualStateMsg);
 void SetPointCallbackLeft(const std_msgs::msg::Float32::SharedPtr setPointMsg);
 void SetPointCallbackRight(const std_msgs::msg::Float32::SharedPtr setPointMsg);

 // PID calculation methods for each wheel
 void PerformPidL();
 void PerformPidR();

 // Other methods
 rcl_interfaces::msg::SetParametersResult ParametersCallback(const std::vector<rclcpp::Parameter> &parameters);
 void DeclareParams();
 void GetParams();
 void PrintParams();
 bool ValidateParams();
 float AlphaFactorCalc(float freq);
};

#endif // PID_LIBRARY_H
pid_library.cpp file:  /**
*  @file     pid_library.cpp
*  @brief    ROS2 library to apply a PID control to a system 
*
*  @author   Antonio Mauro Galiano
*  @details  https://www.linkedin.com/in/antoniomaurogaliano/
*
*/

#include "pid_library/pid_library.h"

PidLibrary::PidLibrary() : Node("ros2_pid_library")
{
 RCLCPP_INFO_STREAM(this->get_logger(), "PID library initialized");

 DeclareParams();
 GetParams();
 PrintParams();
 if (not ValidateParams())
     RCLCPP_ERROR_STREAM(this->get_logger(), "ERROR: Change the parameters");

 paramCallbackHandle_ = this->add_on_set_parameters_callback(
   std::bind(&PidLibrary::ParametersCallback, this, std::placeholders::_1));

 isTheSystemChangedR_ = false;
 isTheSystemChangedL_ = false;


 // Creating publishers for left and right control values
 controlValuePubL_ = this->create_publisher<std_msgs::msg::Float32>(controlValueTopicL_, 1);
 controlValuePubR_ = this->create_publisher<std_msgs::msg::Float32>(controlValueTopicR_, 1);


 actualStateSubR_ = this->create_subscription<std_msgs::msg::Float32>(
     actualStateTopicR_, 10, std::bind(&PidLibrary::ActualStateCallbackRight,
     this, std::placeholders::_1));
 setPointSubR_ = this->create_subscription<std_msgs::msg::Float32>(
     setPointTopicR_, 10, std::bind(&PidLibrary::SetPointCallbackRight,
     this, std::placeholders::_1));  

 actualStateSubL_ = this->create_subscription<std_msgs::msg::Float32>(
     actualStateTopicL_, 10, std::bind(&PidLibrary::ActualStateCallbackLeft,
     this, std::placeholders::_1));
 setPointSubL_ = this->create_subscription<std_msgs::msg::Float32>(
     setPointTopicL_, 10, std::bind(&PidLibrary::SetPointCallbackLeft,
     this, std::placeholders::_1));  
}

PidLibrary::~PidLibrary()
{
 RCLCPP_INFO_STREAM(this->get_logger(), "PID library killed");
}

void PidLibrary::DeclareParams()
{
 RCLCPP_INFO_STREAM(this->get_logger(), "Declaring PID library parameters");

 // PID parameters for left wheel
 this->declare_parameter<float>("kp_left", 0.0);
 this->declare_parameter<float>("ki_left", 0.0);
 this->declare_parameter<float>("kd_left", 0.0);

 // PID parameters for right wheel
 this->declare_parameter<float>("kp_right", 0.0);
 this->declare_parameter<float>("ki_right", 0.0);
 this->declare_parameter<float>("kd_right", 0.0);

 // General parameters
 this->declare_parameter<bool>("use_sample_time", false);
 this->declare_parameter<int>("sample_time", 5);
 this->declare_parameter<bool>("derivative_on_measurement", false);
 this->declare_parameter<bool>("remove_ki_bump", false);
 this->declare_parameter<bool>("reset_windup", false);
 this->declare_parameter<bool>("pid_enabled", true);
 this->declare_parameter<bool>("make_reverse", false);

 // Output limits for left wheel
 this->declare_parameter<int>("out_min_left", 0);
 this->declare_parameter<int>("out_max_left", 0);

 // Output limits for right wheel
 this->declare_parameter<int>("out_min_right", 0);
 this->declare_parameter<int>("out_max_right", 0);

 // Topics for left wheel
 this->declare_parameter<std::string>("control_value_topic_left", "/control_value_topic_left");
 this->declare_parameter<std::string>("actual_state_topic_left", "/actual_state_topic_left");
 this->declare_parameter<std::string>("set_point_topic_left", "/set_point_topic_left");

 // Topics for right wheel
 this->declare_parameter<std::string>("control_value_topic_right", "/control_value_topic_right");
 this->declare_parameter<std::string>("actual_state_topic_right", "/actual_state_topic_right");
 this->declare_parameter<std::string>("set_point_topic_right", "/set_point_topic_right");
}



void PidLibrary::GetParams()
{
 RCLCPP_INFO_STREAM(this->get_logger(), "Getting PID library parameters");

 // PID parameters for left wheel
 this->get_parameter("kp_left", kp_left_);
 this->get_parameter("ki_left", ki_left_);
 this->get_parameter("kd_left", kd_left_);

 // PID parameters for right wheel
 this->get_parameter("kp_right", kp_right_);
 this->get_parameter("ki_right", ki_right_);
 this->get_parameter("kd_right", kd_right_);

 this->get_parameter("use_sample_time", useSampleTime_);
 this->get_parameter("sample_time", sampleTime_);
 this->get_parameter("derivative_on_measurement", derivativeOnMeasurement_);
 this->get_parameter("remove_ki_bump", removeKiBump_);
 this->get_parameter("reset_windup", resetWindup_);
 this->get_parameter("pid_enabled", pidEnabled_);
 this->get_parameter("make_reverse", makeReverse_);


 // Output limits for left wheel
 this->get_parameter("out_min_left", outMinLeft_);
 this->get_parameter("out_max_left", outMaxLeft_);

 // Output limits for right wheel
 this->get_parameter("out_min_right", outMinRight_);
 this->get_parameter("out_max_right", outMaxRight_);


 // Topics for left wheel
 this->get_parameter("control_value_topic_left", controlValueTopicL_);
 this->get_parameter("actual_state_topic_left", actualStateTopicL_);
 this->get_parameter("set_point_topic_left", setPointTopicL_);

 // Topics for right wheel
 this->get_parameter("control_value_topic_right", controlValueTopicR_);
 this->get_parameter("actual_state_topic_right", actualStateTopicR_);
 this->get_parameter("set_point_topic_right", setPointTopicR_);

 RCLCPP_INFO_STREAM(this->get_logger(), "Retrieved PID parameters:");
 RCLCPP_INFO_STREAM(this->get_logger(), "Left wheel: kp=" << kp_left_ << ", ki=" << ki_left_ << ", kd=" << kd_left_);
 RCLCPP_INFO_STREAM(this->get_logger(), "Right wheel: kp=" << kp_right_ << ", ki=" << ki_right_ << ", kd=" << kd_right_);

}

void PidLibrary::PrintParams()
{
   RCLCPP_INFO_STREAM(this->get_logger(), "Printing PID library parameters");

   // PID parameters for left wheel
   RCLCPP_INFO_STREAM(this->get_logger(), "kp_left = " << kp_left_);
   RCLCPP_INFO_STREAM(this->get_logger(), "ki_left = " << ki_left_);
   RCLCPP_INFO_STREAM(this->get_logger(), "kd_left = " << kd_left_);

   // PID parameters for right wheel
   RCLCPP_INFO_STREAM(this->get_logger(), "kp_right = " << kp_right_);
   RCLCPP_INFO_STREAM(this->get_logger(), "ki_right = " << ki_right_);
   RCLCPP_INFO_STREAM(this->get_logger(), "kd_right = " << kd_right_);

   // General parameters
   RCLCPP_INFO_STREAM(this->get_logger(), "use_sample_time = " << useSampleTime_);
   RCLCPP_INFO_STREAM(this->get_logger(), "sample_time = " << sampleTime_);
   RCLCPP_INFO_STREAM(this->get_logger(), "derivative_on_measurement = " << derivativeOnMeasurement_);
   RCLCPP_INFO_STREAM(this->get_logger(), "remove_ki_bump = " << removeKiBump_);
   RCLCPP_INFO_STREAM(this->get_logger(), "reset_windup = " << resetWindup_);
   RCLCPP_INFO_STREAM(this->get_logger(), "pid_enabled = " << pidEnabled_);
   RCLCPP_INFO_STREAM(this->get_logger(), "make_reverse = " << makeReverse_);

   // Output limits for left wheel
   RCLCPP_INFO_STREAM(this->get_logger(), "out_min_left = " << outMinLeft_);
   RCLCPP_INFO_STREAM(this->get_logger(), "out_max_left = " << outMaxLeft_);

   // Output limits for right wheel
   RCLCPP_INFO_STREAM(this->get_logger(), "out_min_right = " << outMinRight_);
   RCLCPP_INFO_STREAM(this->get_logger(), "out_max_right = " << outMaxRight_);

   // Topics for left wheel
   RCLCPP_INFO_STREAM(this->get_logger(), "control_value_topic_left = " << controlValueTopicL_);
   RCLCPP_INFO_STREAM(this->get_logger(), "actual_state_topic_left = " << actualStateTopicL_);
   RCLCPP_INFO_STREAM(this->get_logger(), "set_point_topic_left = " << setPointTopicL_);

   // Topics for right wheel
   RCLCPP_INFO_STREAM(this->get_logger(), "control_value_topic_right = " << controlValueTopicR_);
   RCLCPP_INFO_STREAM(this->get_logger(), "actual_state_topic_right = " << actualStateTopicR_);
   RCLCPP_INFO_STREAM(this->get_logger(), "set_point_topic_right = " << setPointTopicR_);
}



bool PidLibrary::ValidateParams()
{
   // Validate output limits for left wheel
   if (outMinLeft_ > outMaxLeft_)
   {
       RCLCPP_ERROR_STREAM(this->get_logger(), "Left wheel: Min output value can't be greater than the max output value!");
       return false;
   }

   // Validate output limits for right wheel
   if (outMinRight_ > outMaxRight_)
   {
       RCLCPP_ERROR_STREAM(this->get_logger(), "Right wheel: Min output value can't be greater than the max output value!");
       return false;
   }

   // Validate PID gains for left wheel
   if (!((kp_left_ <= 0.0 && ki_left_ <= 0.0 && kd_left_ <= 0.0) || (kp_left_ >= 0.0 && ki_left_ >= 0.0 && kd_left_ >= 0.0)))
   {
       RCLCPP_ERROR_STREAM(this->get_logger(), "Left wheel: All three gains should have the same sign");
       return false;
   }

   // Validate PID gains for right wheel
   if (!((kp_right_ <= 0.0 && ki_right_ <= 0.0 && kd_right_ <= 0.0) || (kp_right_ >= 0.0 && ki_right_ >= 0.0 && kd_right_ >= 0.0)))
   {
       RCLCPP_ERROR_STREAM(this->get_logger(), "Right wheel: All three gains should have the same sign");
       return false;
   }

   return true;
}


rcl_interfaces::msg::SetParametersResult PidLibrary::ParametersCallback(
 const std::vector<rclcpp::Parameter> &parameters)
{ 
 RCLCPP_INFO_STREAM(this->get_logger(), "!!!PARAMETER CHANGED!!!");
 rcl_interfaces::msg::SetParametersResult result;
 result.successful = true;
 result.reason = "success";
 for (const auto &param: parameters)
 {
   RCLCPP_INFO_STREAM(this->get_logger(), param.get_name().c_str() << "=" << param.value_to_string().c_str());
   
   // Parameters for the left wheel
   if (param.get_name() == "kp_left")
   {
     kp_left_ = param.as_double();
   }
   else if (param.get_name() == "ki_left")
   {
     ki_left_ = param.as_double();
   }
   else if (param.get_name() == "kd_left")
   {
     kd_left_ = param.as_double();
   }

   // Parameters for the right wheel
   if (param.get_name() == "kp_right")
   {
     kp_right_ = param.as_double();
   }
   else if (param.get_name() == "ki_right")
   {
     ki_right_ = param.as_double();
   }
   else if (param.get_name() == "kd_right")
   {
     kd_right_ = param.as_double();
   }

   // Shared parameters
   if (param.get_name() == "use_sample_time")
   {
     useSampleTime_ = param.as_bool();
     if (useSampleTime_)
     {
       ki_left_ = ki_left_ * sampleTime_;
       kd_left_ = kd_left_ / sampleTime_;
       ki_right_ = ki_right_ * sampleTime_;
       kd_right_ = kd_right_ / sampleTime_;
     }
   }
   else if (param.get_name() == "sample_time")
   {
     if (param.as_int() != sampleTime_ && param.as_int() > 0)
     {
       double ratio = static_cast<double>(param.as_int()) / sampleTime_;
       ki_left_ *= ratio;
       kd_left_ /= ratio;
       ki_right_ *= ratio;
       kd_right_ /= ratio;
       sampleTime_ = param.as_int();
     }
   }
   else if (param.get_name() == "derivative_on_measurement")
   {
     derivativeOnMeasurement_ = param.as_bool();
   }
   else if (param.get_name() == "remove_ki_bump")
   {
     removeKiBump_ = param.as_bool();
   }
   else if (param.get_name() == "reset_windup")
   {
     resetWindup_ = param.as_bool();
   }
   else if (param.get_name() == "out_min_left")
   {
     outMinLeft_ = param.as_int();
     // Check and adjust control values for the left wheel
     if(controlValueL_ < outMinLeft_) controlValueL_ = outMinLeft_;
     if(errorIntegralLeft_ < outMinLeft_) errorIntegralLeft_ = outMinLeft_;
   } 
   else if (param.get_name() == "out_min_right")
   {
     outMinRight_ = param.as_int();
     // Check and adjust control values for the left wheel
     if(controlValueR_ < outMinRight_) controlValueR_ = outMinRight_;
     if(errorIntegralRight_ < outMinRight_) errorIntegralRight_ = outMinRight_;
   }

   else if (param.get_name() == "out_max_left")
   {
     outMaxLeft_ = param.as_int();
     // Check and adjust control values for the left wheel
     if(controlValueL_ > outMaxLeft_) controlValueL_ = outMaxLeft_;
     if(errorIntegralLeft_ > outMaxLeft_) errorIntegralLeft_ = outMaxLeft_;
   } 

   else if (param.get_name() == "out_max_right")
   {
     outMaxRight_ = param.as_int();
     // Check and adjust control values for the left wheel
     if(controlValueR_ > outMaxRight_) controlValueR_ = outMaxRight_;
     if(errorIntegralRight_ > outMaxRight_) errorIntegralRight_ = outMaxRight_;
   }

   else if (param.get_name() == "pid_enabled")
   {
     pidEnabled_ = param.as_bool();
     if (pidEnabled_)
     {
       oldActualStateValueR_ = actualStateValueR_;
       oldActualStateValueL_ = actualStateValueL_;
       errorIntegralLeft_ = controlValueL_;
       errorIntegralRight_ = controlValueR_;
       if(errorIntegralLeft_ > outMaxLeft_) errorIntegralLeft_ = outMaxLeft_;
       else if(errorIntegralLeft_ < outMinLeft_) errorIntegralLeft_ = outMinLeft_;
       if(errorIntegralRight_ > outMaxRight_) errorIntegralRight_ = outMaxRight_;
       else if(errorIntegralRight_ < outMinRight_) errorIntegralRight_ = outMinRight_;
     }
   }
   else if (param.get_name() == "make_reverse")
   {
     makeReverse_ = param.as_bool();
     if (makeReverse_)
     {
         kp_left_ = -kp_left_;
         ki_left_ = -ki_left_;
         kd_left_ = -kd_left_;
         kp_right_ = -kp_right_;
         ki_right_ = -ki_right_;
         kd_right_ = -kd_right_;
     }
   }
   if (not ValidateParams())
     RCLCPP_ERROR_STREAM(this->get_logger(), "ERROR: Change the parameters");
 }
 return result;
}


void PidLibrary::ActualStateCallbackLeft(const std_msgs::msg::Float32::SharedPtr actualStateMsg)
{
   actualStateValueL_ = actualStateMsg->data;
   isTheSystemChangedL_ = true;
   PerformPidL();
}

void PidLibrary::ActualStateCallbackRight(const std_msgs::msg::Float32::SharedPtr actualStateMsg)
{
   actualStateValueR_ = actualStateMsg->data;
   RCLCPP_DEBUG(this->get_logger(), "Received actual state right: %f", actualStateValueR_);
   isTheSystemChangedR_ = true;
   PerformPidR();
}

void PidLibrary::SetPointCallbackLeft(const std_msgs::msg::Float32::SharedPtr setPointMsg)
{
   setPointValueL_ = setPointMsg->data;
   isTheSystemChangedL_ = true; // Flag to indicate the left system has changed
}

void PidLibrary::SetPointCallbackRight(const std_msgs::msg::Float32::SharedPtr setPointMsg)
{
   setPointValueR_ = setPointMsg->data;
   isTheSystemChangedR_ = true; // Flag to indicate the right system has changed
}


float PidLibrary::AlphaFactorCalc(float freq)
{
 if (freq <= 0)
   return 1;
 const float cosine = std::cos(2 * float(M_PI) * freq);
 return cosine - 1 + std::sqrt( (cosine * cosine) - (4 * cosine) + 3 );
}



void PidLibrary::PerformPidR()
{
   if (pidEnabled_ && isTheSystemChangedR_)
   {
       if (previousTime_.seconds() != 0.0)
       { 
           currentTime_ = this->get_clock()->now();
           deltaTime_ = currentTime_ - previousTime_;
           if (deltaTime_.seconds() == 0)
           {
               RCLCPP_ERROR(this->get_logger(), "Delta Time is 0, skipping this loop.");
           }
       }
       else
       {
           previousTime_ = this->get_clock()->now();
      
       }
       float errorR = setPointValueR_ - actualStateValueR_;
       float errorDerivativeR = 0.0;


       if (useSampleTime_ && deltaTime_.seconds() >= sampleTime_)
       {
           errorIntegralRight_ += (removeKiBump_ ? ki_right_ * errorR : errorR);
           errorDerivativeR = derivativeOnMeasurement_ ? (actualStateValueR_ - oldActualStateValueR_) : (errorR - oldErrorR_);
       }
       else if (!useSampleTime_)
       {

           errorIntegralRight_ += (removeKiBump_ ? ki_right_ * errorR : errorR * deltaTime_.seconds());
           // Modified code to check deltaTime_.seconds() is greater than zero
           if (deltaTime_.seconds() > 0) {
               errorDerivativeR = derivativeOnMeasurement_ ?
                                  (actualStateValueR_ - oldActualStateValueR_) / deltaTime_.seconds() :
                                  (errorR - oldErrorR_) / deltaTime_.seconds();
           } else {
               RCLCPP_ERROR(this->get_logger(), "Delta Time is 0 at Right, error derivative calculation skipped.");
           }
         
       }

       if (resetWindup_)
       {
         errorIntegralRight_ = std::clamp(errorIntegralRight_, static_cast<float>(outMinRight_), static_cast<float>(outMaxRight_));
       }

       controlValueR_ = kp_right_ * errorR + ki_right_ * errorIntegralRight_ + (derivativeOnMeasurement_ ? -kd_right_ * errorDerivativeR : kd_right_ * errorDerivativeR);
       controlValueR_ = std::clamp(controlValueR_, static_cast<float>(outMinRight_), static_cast<float>(outMaxRight_));

       oldErrorR_ = errorR;
       oldActualStateValueR_ = actualStateValueR_;
       previousTime_ = currentTime_;

       RCLCPP_INFO(this->get_logger(), "Set Point Right=%f | Control Value Right=%f | Actual State Right=%f", setPointValueR_, controlValueR_, actualStateValueR_);

       
       if (std::isnan(controlValueR_)) {
           RCLCPP_ERROR(this->get_logger(), "Control Value Right is NaN");
       } else {
           std_msgs::msg::Float32 controlValueMsgR;
           controlValueMsgR.data = controlValueR_;
           controlValuePubR_->publish(controlValueMsgR);
       }
       isTheSystemChangedR_ = false;

       RCLCPP_DEBUG(this->get_logger(), "Right wheel PID calculations:");
       RCLCPP_DEBUG(this->get_logger(), "Error: %f, Integral: %f, Derivative: %f", errorR, errorIntegralRight_, errorDerivativeR);

       // After clamping and final control value calculation
       RCLCPP_DEBUG(this->get_logger(), "Clamped Integral: %f, Control Value: %f", errorIntegralRight_, controlValueR_);
   }



}




void PidLibrary::PerformPidL()
{
   if (pidEnabled_ && isTheSystemChangedL_)
   {
       if (previousTime_.seconds() != 0.0)
       { 
           currentTime_ = this->get_clock()->now();
           deltaTime_ = currentTime_ - previousTime_;
           if (deltaTime_.seconds() == 0)
           {
               RCLCPP_ERROR(this->get_logger(), "Delta Time is 0, skipping this loop.");

           }
       }
       else
       {
           previousTime_ = this->get_clock()->now();
       }

       float errorL = setPointValueL_ - actualStateValueL_;
       float errorDerivativeL = 0.0;

       if (useSampleTime_ && deltaTime_.seconds() >= sampleTime_)
       {
           errorIntegralLeft_ += (removeKiBump_ ? ki_left_ * errorL : errorL);
           errorDerivativeL = derivativeOnMeasurement_ ? (actualStateValueL_ - oldActualStateValueL_) : (errorL - oldErrorL_);
       }
       else if (!useSampleTime_)
       {
           errorIntegralLeft_ += (removeKiBump_ ? ki_left_ * errorL : errorL * deltaTime_.seconds());
           if (deltaTime_.seconds() > 0) {
               errorDerivativeL = derivativeOnMeasurement_ ?
                                  (actualStateValueL_ - oldActualStateValueL_) / deltaTime_.seconds() :
                                  (errorL - oldErrorL_) / deltaTime_.seconds();
           } else {
               RCLCPP_ERROR(this->get_logger(), "Delta Time is 0 at Left wheel, error derivative calculation skipped.");
           }

       }

       if (resetWindup_)
       { 
         errorIntegralLeft_ = std::clamp(errorIntegralLeft_, static_cast<float>(outMinLeft_), static_cast<float>(outMaxLeft_));
       }

       controlValueL_ = kp_left_ * errorL + ki_left_ * errorIntegralLeft_ + (derivativeOnMeasurement_ ? -kd_left_ * errorDerivativeL : kd_left_ * errorDerivativeL);
       controlValueL_ = std::clamp(controlValueL_, static_cast<float>(outMinLeft_), static_cast<float>(outMaxLeft_));

       oldErrorL_ = errorL;
       oldActualStateValueL_ = actualStateValueL_;
       previousTime_ = currentTime_;
       RCLCPP_INFO(this->get_logger(), "Set Point Right=%f | Control Value Right=%f | Actual State Right=%f", setPointValueL_, controlValueL_, actualStateValueL_);

       std_msgs::msg::Float32 controlValueMsgL;
       controlValueMsgL.data = controlValueL_;
       controlValuePubL_->publish(controlValueMsgL);

       isTheSystemChangedL_ = false;
   }
}

void PidLibrary::UsePidLibrary()
{
 PerformPidL();
 PerformPidR();
}

example_system.cpp file:

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32.hpp"


class ExampleSystem : public rclcpp::Node
{
public:
  ExampleSystem() : Node("pid_example_system")
    {
      RCLCPP_INFO_STREAM(this->get_logger(), "Echo line from pid_example_system");
      this->declare_parameter<int>("plant_order", 1);


      // Subscribe to control value topics for both wheels
      controlValueSubLeft_ = this->create_subscription<std_msgs::msg::Float32>(
        "/control_value_topic_left", 10, std::bind(&ExampleSystem::ControlValueCallbackLeft, this, std::placeholders::_1));

      controlValueSubRight_ = this->create_subscription<std_msgs::msg::Float32>(
        "/control_value_topic_right", 10, std::bind(&ExampleSystem::ControlValueCallbackRight, this, std::placeholders::_1));

            // Publish actual state topics for both wheels
      actualStatePubLeft_ = this->create_publisher<std_msgs::msg::Float32>("/actual_state_topic_left", 10);
      actualStatePubRight_ = this->create_publisher<std_msgs::msg::Float32>("/actual_state_topic_right", 10);

    
      actualStatePubLeft_->publish(plantStateLeft_);
      actualStatePubRight_->publish(plantStateRight_);
    }


  std_msgs::msg::Float32 plantStateLeft_, plantStateRight_;
  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr actualStatePubLeft_, actualStatePubRight_;
  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr controlValueSubLeft_, controlValueSubRight_;

private:
  int plantOrder_ = 1;
  float tempLeft_ = 4.7, tempRight_ = 4.7;
  float displacementLeft_ = 3.3, displacementRight_ = 3.3;
  float deltaT_ = 0.01;

  // Process variables
  float speedLeft_ = 0.0, speedRight_ = 0.0;
  float accelerationLeft_ = 0.0, accelerationRight_ = 0.0;
  float mass_ = 0.1;
  float friction_ = 1.0;
  float stiction_ = 1.0;
  float kV_ = 1.0;
  float kBackemf_ = 0.0;
  float decelForceLeft_, decelForceRight_;
  float controlEffortLeft_, controlEffortRight_;


  void ControlValueCallbackLeft(const std_msgs::msg::Float32::SharedPtr msg) {
    controlEffortLeft_ = msg->data;
    ProcessPlant(plantOrder_, tempLeft_, displacementLeft_, speedLeft_, accelerationLeft_, decelForceLeft_, controlEffortLeft_, true);
  }

  void ControlValueCallbackRight(const std_msgs::msg::Float32::SharedPtr msg) {
    controlEffortRight_ = msg->data;
    ProcessPlant(plantOrder_, tempRight_, displacementRight_, speedRight_, accelerationRight_, decelForceRight_, controlEffortRight_, false);
  }

void ProcessPlant(int plantOrder, float &temp, float &displacement, float &speed, float &acceleration, float &decelForce, float controlEffort, bool isLeft) {
    switch (plantOrder) {
      case 1: // First order plant
        temp += (0.1 * temp + controlEffort) * deltaT_;
        break;

      case 2: // Second order plant
        if (fabs(speed) < 0.001 && fabs(controlEffort) < stiction_) {
          speed = 0.0;  // Apply stiction effect
        }
        decelForce = -(speed * friction_);
        acceleration = (kV_ * (controlEffort - (kBackemf_ * speed)) + decelForce) / mass_;
        speed += acceleration * deltaT_;
        displacement += speed * deltaT_;
        break;

      default:
        RCLCPP_ERROR(this->get_logger(), "Invalid plant_order");
        break;
    }

    std_msgs::msg::Float32 stateMsg;
    stateMsg.data = (plantOrder == 1) ? temp : displacement;
    if (isLeft) {
      actualStatePubLeft_->publish(stateMsg);
    } else {
      actualStatePubRight_->publish(stateMsg);
    }
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ExampleSystem>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

example_sys_launch.py file:


from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='use_library',
            executable='use_library',
            name='use_library',
            output='screen',
            parameters=[
                # PID parameters for left wheel
                {'kp_left' : 5.0},
                {'ki_left' : 0.0},
                {'kd_left' : 0.1},

                # PID parameters for right wheel
                {'kp_right' : 5.0},
                {'ki_right' : 0.0},
                {'kd_right' : 0.1},

                {'use_sample_time' : False},
                {'sample_time' : 2},
                {'derivative_on_measurement' : False},
                {'remove_ki_bump' : False},
                {'reset_windup' : False},
                {'pid_enabled' : True},

                # Output limits for left wheel
                {'out_min_left' : -50},
                {'out_max_left' : 50},

                # Output limits for right wheel
                {'out_min_right' : -50},
                {'out_max_right' : 50},

                # Topics for left wheel
                {'control_value_topic_left' : '/control_value_topic_left'},
                {'actual_state_topic_left' : '/actual_state_topic_left'},
                {'set_point_topic_left' : '/set_point_topic_left'},

                # Topics for right wheel
                {'control_value_topic_right' : '/control_value_topic_right'},
                {'actual_state_topic_right' : '/actual_state_topic_right'},
                {'set_point_topic_right' : '/set_point_topic_right'},

                {'loop_freq' : 10}
            ]
        ),
        Node(
            package='example_system',
            executable='example_system',
            name='example_system',
            output='screen'
        )
    ])

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.