From 4cadb68fd9012df702bdee8094a76822fd4bfe74 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 17:02:57 +0000 Subject: [PATCH] Replaced member variables with constants. --- include/path_tracking_pid/controller.hpp | 15 ------ src/controller.cpp | 61 +++++++++++++++--------- 2 files changed, 38 insertions(+), 38 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index fcd3488d..3c8db60c 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -308,21 +308,6 @@ class Controller double vel_max_external_ = INFINITY; // Dynamic external max velocity requirement (e.g. no more power available) double vel_max_obstacle_ = INFINITY; // Can be zero if lethal obstacles are detected - // Cutoff frequency for the derivative calculation in Hz. - // Negative -> Has not been set by the user yet, so use a default. - double cutoff_frequency_lat_ = -1.0; - double cutoff_frequency_ang_ = -1.0; - - // Upper and lower saturation limits - double upper_limit_ = 100.0; - double lower_limit_ = -100.0; - - double ang_upper_limit_ = 100.0; - double ang_lower_limit_ = -100.0; - - // Anti-windup term. Limits the absolute value of the integral term. - double windup_limit_ = 1000.0; - // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at // 1/4 of the sample rate. double c_lat_ = 1.; diff --git a/src/controller.cpp b/src/controller.cpp index 2ea4d88f..6e576934 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -20,6 +20,21 @@ namespace { constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) +// Cutoff frequency for the derivative calculation in Hz. +constexpr double cutoff_frequency_lat = -1.0; +constexpr double cutoff_frequency_ang = -1.0; + +// Upper and lower saturation limits +constexpr double lat_upper_limit = 100.0; +constexpr double lat_lower_limit = -100.0; + +constexpr double ang_upper_limit = 100.0; +constexpr double ang_lower_limit = -100.0; + +// Anti-windup term. Limits the absolute value of the integral term. +constexpr double windup_limit = 1000.0; + + // Typesafe sign implementation with signum: // https://stackoverflow.com/a/4609795 template int sign(T val) { @@ -501,20 +516,20 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, controller_state_.error_integral_ang += controller_state_.error_ang.at(0) * dt.toSec(); // Apply windup limit to limit the size of the integral term - if (controller_state_.error_integral_lat > std::abs(windup_limit_)) - controller_state_.error_integral_lat = std::abs(windup_limit_); - if (controller_state_.error_integral_lat < -std::abs(windup_limit_)) - controller_state_.error_integral_lat = -std::abs(windup_limit_); - if (controller_state_.error_integral_ang > std::abs(windup_limit_)) - controller_state_.error_integral_ang = std::abs(windup_limit_); - if (controller_state_.error_integral_ang < -std::abs(windup_limit_)) - controller_state_.error_integral_ang = -std::abs(windup_limit_); + if (controller_state_.error_integral_lat > std::abs(windup_limit)) + controller_state_.error_integral_lat = std::abs(windup_limit); + if (controller_state_.error_integral_lat < -std::abs(windup_limit)) + controller_state_.error_integral_lat = -std::abs(windup_limit); + if (controller_state_.error_integral_ang > std::abs(windup_limit)) + controller_state_.error_integral_ang = std::abs(windup_limit); + if (controller_state_.error_integral_ang < -std::abs(windup_limit)) + controller_state_.error_integral_ang = -std::abs(windup_limit); // My filter reference was Julius O. Smith III, Intro. to Digital Filters With Audio Applications. - if (cutoff_frequency_lat_ != -1) + if (cutoff_frequency_lat != -1) { // Check if tan(_) is really small, could cause c = NaN - auto tan_filt = tan((cutoff_frequency_lat_ * 6.2832) * dt.toSec() / 2); + auto tan_filt = tan((cutoff_frequency_lat * 6.2832) * dt.toSec() / 2); // Avoid tan(0) ==> NaN if ((tan_filt <= 0.) && (tan_filt > -0.01)) @@ -524,10 +539,10 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, c_lat_ = 1 / tan_filt; } - if (cutoff_frequency_ang_ != -1) + if (cutoff_frequency_ang != -1) { // Check if tan(_) is really small, could cause c = NaN - auto tan_filt = tan((cutoff_frequency_ang_ * 6.2832) * dt.toSec() / 2); + auto tan_filt = tan((cutoff_frequency_ang * 6.2832) * dt.toSec() / 2); // Avoid tan(0) ==> NaN if ((tan_filt <= 0.) && (tan_filt > -0.01)) @@ -773,15 +788,15 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, // Apply saturation limits - if (control_effort_lat_ > upper_limit_) - control_effort_lat_ = upper_limit_; - else if (control_effort_lat_ < lower_limit_) - control_effort_lat_ = lower_limit_; + if (control_effort_lat_ > lat_upper_limit) + control_effort_lat_ = lat_upper_limit; + else if (control_effort_lat_ < lat_lower_limit) + control_effort_lat_ = lat_lower_limit; - if (control_effort_ang_ > ang_upper_limit_) - control_effort_ang_ = ang_upper_limit_; - else if (control_effort_ang_ < ang_lower_limit_) - control_effort_ang_ = ang_lower_limit_; + if (control_effort_ang_ > ang_upper_limit) + control_effort_ang_ = ang_upper_limit; + else if (control_effort_ang_ < ang_lower_limit) + control_effort_ang_ = ang_lower_limit; // Populate debug output // Error topic containing the 'control' error on which the PID acts @@ -1130,9 +1145,9 @@ void Controller::printParameters() ROS_INFO("Robot type (holonomic): (%i)", holonomic_robot_enable_); - ROS_INFO("Integral-windup limit: %f", windup_limit_); - ROS_INFO("Saturation limits xy: %f/%f", upper_limit_, lower_limit_); - ROS_INFO("Saturation limits ang: %f/%f", ang_upper_limit_, ang_lower_limit_); + ROS_INFO("Integral-windup limit: %f", windup_limit); + ROS_INFO("Saturation limits xy: %f/%f", lat_upper_limit, lat_lower_limit); + ROS_INFO("Saturation limits ang: %f/%f", ang_upper_limit, ang_lower_limit); ROS_INFO("-----------------------------------------"); }