Skip to content

Commit

Permalink
Merge pull request #18 from uri-ocean-robotics/noetic-devel-integral-…
Browse files Browse the repository at this point in the history 
…windup

Noetic devel integral windup
  • Loading branch information
@mzhouURI
mzhouURI authored Oct 27, 2024
2 parents 85bf859 + 5e547ea commit 8b10fe3
Show file tree
Hide file tree
Showing 6 changed files with 210 additions and 251 deletions.
4 changes: 2 additions & 2 deletions cfg/dynamic_reconfigure_pid.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ for i in axis:
gen.add(i + "_p", double_t, 0, "Controller " + i + " PID (P) Gain", 1, 0, 200)
gen.add(i + "_i", double_t, 0, "Controller " + i + " PID (I) Gain", 0, 0, 200)
gen.add(i + "_d", double_t, 0, "Controller " + i + " PID (D) Gain", 0, 0, 200)
gen.add(i + "_i_max", double_t, 0, "Controller " + i + " PID max (i) term", 0, -50, 50)
gen.add(i + "_i_min", double_t, 0, "Controller " + i + " PID min (i) term", 0, -50, 50)
gen.add(i + "_pid_max", double_t, 0, "Controller " + i + " PID max term", 0, -50, 50)
gen.add(i + "_pid_min", double_t, 0, "Controller " + i + " PID min term", 0, -50, 50)

exit(gen.generate(PACKAGE, "mvp_control_dynamic_configure", "PID"))
8 changes: 6 additions & 2 deletions include/mvp_control/dictionary.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,8 +70,12 @@ namespace ctrl {
static constexpr const char * CONF_PID_P = "p";
static constexpr const char * CONF_PID_I = "i";
static constexpr const char * CONF_PID_D = "d";
static constexpr const char * CONF_PID_I_MAX = "i_max";
static constexpr const char * CONF_PID_I_MIN = "i_min";
// static constexpr const char * CONF_PID_I_MAX = "i_max";
// static constexpr const char * CONF_PID_I_MIN = "i_min";

static constexpr const char * CONF_PID_MAX = "pid_max";
static constexpr const char * CONF_PID_MIN = "pid_min";

static constexpr const char * CONF_CONTROL_ALLOCATION_MATRIX = "control_allocation_matrix";
static constexpr const char * CONF_CONTROL_TF = "control_tf";
static constexpr const char * CONF_CONTROLLER_FREQUENCY = "controller_frequency";
Expand Down
53 changes: 35 additions & 18 deletions src/mvp_control/mimo_pid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
using namespace ctrl;

MimoPID::MimoPID() : m_error_function(nullptr) , m_dt_i(10000){

m_i.setZero();
}

bool MimoPID::calculate(Eigen::VectorXd* u, const Eigen::ArrayXd& desired, const Eigen::ArrayXd& current, double dt) {
Expand All @@ -40,15 +40,15 @@ bool MimoPID::calculate(Eigen::VectorXd* u, const Eigen::ArrayXd& desired, const

if(m_i.size() == 0) {
m_i.resize(error.size());
// printf("errorsize = %d\r\n", error.size());
}

// Proportional term
Eigen::ArrayXd p = m_kp * error;


m_i += m_ki * (error * dt);

m_i = (m_i > m_i_max).select(m_i_max, m_i);
m_i = (m_i < m_i_min).select(m_i_min, m_i);
Eigen::ArrayXd delta_i;
delta_i = m_ki * (error * dt);

// Derivation term
if(!m_pe.data()) {
Expand All @@ -60,7 +60,23 @@ bool MimoPID::calculate(Eigen::VectorXd* u, const Eigen::ArrayXd& desired, const

m_pe = error;

*u = p + m_i + d;
Eigen::ArrayXd pid_sum = p + m_i + d;

//saturation
pid_sum = (pid_sum > m_pid_max).select(m_pid_max, pid_sum);
pid_sum = (pid_sum < m_pid_min).select(m_pid_min, pid_sum);

//clamping only update the integral when not saturated
m_i = (pid_sum.array() > m_pid_min.array() && pid_sum.array() < m_pid_max.array())
.select(m_i + delta_i, m_i);
// *u = p + m_i + d;
*u = pid_sum;

// for(int i = 0; i<m_i.size(); i++)
// {
// printf("integration[%d]=%lf, total =%lf\r\n", i, m_i[i], pid_sum[i]);

// }

return true;
}
Expand Down Expand Up @@ -105,28 +121,29 @@ void MimoPID::set_dt_i(const decltype(m_dt_i) &gain) {
m_dt_i = gain;
}

auto MimoPID::get_i_max() -> decltype(m_i_max) {
return m_i_max;
auto MimoPID::get_pid_max() -> decltype(m_pid_max) {
return m_pid_max;
}

void MimoPID::set_i_max(const decltype(m_i_max) &gain) {
m_i_max= gain;
void MimoPID::set_pid_max(const decltype(m_pid_max) &gain) {
m_pid_max= gain;
}

auto MimoPID::get_i_min() -> decltype(m_i_max) {
return m_i_min;
auto MimoPID::get_pid_min() -> decltype(m_pid_min) {
return m_pid_min;
}

void MimoPID::reset_m_i(const decltype(m_i)) {
m_i.setZero();
void MimoPID::set_pid_min(const decltype(m_pid_min) &gain) {
m_pid_min= gain;
}

auto MimoPID::get_m_i() -> decltype(m_i) {
return m_i;
void MimoPID::set_m_i(const decltype(m_i) &new_m_i)
{
m_i = new_m_i;
}

void MimoPID::set_i_min(const decltype(m_i_min) &gain) {
m_i_min= gain;
auto MimoPID::get_m_i() -> decltype(m_i) {
return m_i;
}

auto MimoPID::get_error_function() -> decltype(m_error_function) {
Expand Down
14 changes: 7 additions & 7 deletions src/mvp_control/mimo_pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,14 +50,14 @@ namespace ctrl {
*
* This variable holds the maximum value of a gain
*/
Eigen::ArrayXd m_i_max;
Eigen::ArrayXd m_pid_max;

/**
* @brief Minimum value
*
* This variable holds the minimum value of a gain
*/
Eigen::ArrayXd m_i_min;
Eigen::ArrayXd m_pid_min;

//! @brief Proportional gain
Eigen::ArrayXd m_kp;
Expand Down Expand Up @@ -143,24 +143,24 @@ namespace ctrl {
void set_dt_i(const decltype(m_dt_i) &gain);

//! @brief Default getter for max
auto get_i_max() -> decltype(m_i_max);
auto get_pid_max() -> decltype(m_pid_max);

/*! @brief Default setter for max
*
* @param gain
*/
void set_i_max(const decltype(m_i_max) &gain);
void set_pid_max(const decltype(m_pid_max) &gain);

//! @brief Default getter for min
auto get_i_min() -> decltype(m_i_min);
auto get_pid_min() -> decltype(m_pid_min);

/*! @brief Default setter for min
*
* @param gain
*/
void set_i_min(const decltype(m_i_min) &gain);
void set_pid_min(const decltype(m_pid_min) &gain);

void reset_m_i(const decltype(m_i));
void set_m_i(const decltype(m_i) &new_m_i);

auto get_m_i() -> decltype(m_i);

Expand Down
Loading

0 comments on commit 8b10fe3

Please sign in to comment.