|
|
|
@ -21,7 +21,7 @@ int32_t AC_PID::get_p(int32_t error)
@@ -21,7 +21,7 @@ int32_t AC_PID::get_p(int32_t error)
|
|
|
|
|
|
|
|
|
|
int32_t AC_PID::get_i(int32_t error, float dt) |
|
|
|
|
{ |
|
|
|
|
if((_ki != 0) && (dt != 0)){ |
|
|
|
|
if((_ki != 0) && (dt != 0)) { |
|
|
|
|
_integrator += ((float)error * _ki) * dt; |
|
|
|
|
if (_integrator < -_imax) { |
|
|
|
|
_integrator = -_imax; |
|
|
|
@ -67,42 +67,42 @@ int32_t AC_PID::get_pid(int32_t error, float dt)
@@ -67,42 +67,42 @@ int32_t AC_PID::get_pid(int32_t error, float dt)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
int32_t AC_PID::get_pid(int32_t error, float dt) |
|
|
|
|
{ |
|
|
|
|
// Compute proportional component
|
|
|
|
|
_output = error * _kp; |
|
|
|
|
|
|
|
|
|
// Compute derivative component if time has elapsed
|
|
|
|
|
if ((fabs(_kd) > 0) && (dt > 0)) { |
|
|
|
|
_derivative = (error - _last_error) / dt; |
|
|
|
|
|
|
|
|
|
// discrete low pass filter, cuts out the
|
|
|
|
|
// high frequency noise that can drive the controller crazy
|
|
|
|
|
_derivative = _last_derivative + |
|
|
|
|
(dt / ( _filter + dt)) * (_derivative - _last_derivative); |
|
|
|
|
|
|
|
|
|
// update state
|
|
|
|
|
_last_error = error; |
|
|
|
|
_last_derivative = _derivative; |
|
|
|
|
|
|
|
|
|
// add in derivative component
|
|
|
|
|
_output += _kd * _derivative; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Compute integral component if time has elapsed
|
|
|
|
|
if ((fabs(_ki) > 0) && (dt > 0)) { |
|
|
|
|
_integrator += (error * _ki) * dt; |
|
|
|
|
if (_integrator < -_imax) { |
|
|
|
|
_integrator = -_imax; |
|
|
|
|
} else if (_integrator > _imax) { |
|
|
|
|
_integrator = _imax; |
|
|
|
|
} |
|
|
|
|
_output += _integrator; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return _output; |
|
|
|
|
} |
|
|
|
|
*/ |
|
|
|
|
* int32_t AC_PID::get_pid(int32_t error, float dt) |
|
|
|
|
* { |
|
|
|
|
* // Compute proportional component
|
|
|
|
|
* _output = error * _kp; |
|
|
|
|
* |
|
|
|
|
* // Compute derivative component if time has elapsed
|
|
|
|
|
* if ((fabs(_kd) > 0) && (dt > 0)) { |
|
|
|
|
* _derivative = (error - _last_error) / dt; |
|
|
|
|
* |
|
|
|
|
* // discrete low pass filter, cuts out the
|
|
|
|
|
* // high frequency noise that can drive the controller crazy
|
|
|
|
|
* _derivative = _last_derivative + |
|
|
|
|
* (dt / ( _filter + dt)) * (_derivative - _last_derivative); |
|
|
|
|
* |
|
|
|
|
* // update state
|
|
|
|
|
* _last_error = error; |
|
|
|
|
* _last_derivative = _derivative; |
|
|
|
|
* |
|
|
|
|
* // add in derivative component
|
|
|
|
|
* _output += _kd * _derivative; |
|
|
|
|
* } |
|
|
|
|
* |
|
|
|
|
* // Compute integral component if time has elapsed
|
|
|
|
|
* if ((fabs(_ki) > 0) && (dt > 0)) { |
|
|
|
|
* _integrator += (error * _ki) * dt; |
|
|
|
|
* if (_integrator < -_imax) { |
|
|
|
|
* _integrator = -_imax; |
|
|
|
|
* } else if (_integrator > _imax) { |
|
|
|
|
* _integrator = _imax; |
|
|
|
|
* } |
|
|
|
|
* _output += _integrator; |
|
|
|
|
* } |
|
|
|
|
* |
|
|
|
|
* return _output; |
|
|
|
|
* } |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void |
|
|
|
|