Skip to content

Commit

Permalink
🐛Fixed derivative kick #99 (#105)
Browse files Browse the repository at this point in the history
  • Loading branch information
ssejrog authored Jun 2, 2024
1 parent d844c96 commit 576eb32
Show file tree
Hide file tree
Showing 2 changed files with 42 additions and 13 deletions.
33 changes: 23 additions & 10 deletions include/EZ-Template/PID.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,10 +97,21 @@ class PID {
* Computes PID.
*
* \param current
* Current sensor library.
* Current sensor value.
*/
double compute(double current);

/**
* Computes PID, but you set the error yourself. This function ignores target.
* Current is only used here for calculative derivative.
*
* \param err
* Error in PID, you need to calculate this yourself.
* \param current
* Current sensor value.
*/
double compute_error(double err, double current);

/**
* Returns target value.
*/
Expand Down Expand Up @@ -183,15 +194,16 @@ class PID {
/**
* PID variables.
*/
double output;
double cur;
double error;
double target;
double prev_error;
double integral;
double derivative;
long time;
long prev_time;
double output = 0.0;
double cur = 0.0;
double error = 0.0;
double target = 0.0;
double prev_error = 0.0;
double prev_current = 0.0;
double integral = 0.0;
double derivative = 0.0;
long time = 0;
long prev_time = 0;

private:
int i = 0, j = 0, k = 0, l = 0;
Expand All @@ -201,5 +213,6 @@ class PID {
bool name_active = false;
void exit_condition_print(ez::exit_output exit_type);
bool reset_i_sgn = true;
double raw_compute();
};
}; // namespace ez
22 changes: 19 additions & 3 deletions src/EZ-Template/PID.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -59,18 +59,34 @@ bool PID::i_reset_get() { return reset_i_sgn; };

double PID::compute(double current) {
error = target - current;
derivative = error - prev_error;
return compute_error(error, current);
}

double PID::compute_error(double err, double current) {
error = err;
cur = current;

return raw_compute();
}

double PID::raw_compute() {
// calculate derivative on measurement instead of error to avoid "derivative kick"
// https://www.isa.org/intech-home/2023/june-2023/features/fundamentals-pid-control
derivative = cur - prev_current;

if (constants.ki != 0) {
// Only compute i when within a threshold of target
if (fabs(error) < constants.start_i)
integral += error;

if (util::sgn(error) != util::sgn(prev_error) && reset_i_sgn)
// Reset i when the sign of error flips
if (util::sgn(error) != util::sgn(prev_current) && reset_i_sgn)
integral = 0;
}

output = (error * constants.kp) + (integral * constants.ki) + (derivative * constants.kd);
output = (error * constants.kp) + (integral * constants.ki) - (derivative * constants.kd);

prev_current = cur;
prev_error = error;

return output;
Expand Down

0 comments on commit 576eb32

Please sign in to comment.