forked from logzhan/RobotHardware-UESTC
71 lines
1.7 KiB
C++
71 lines
1.7 KiB
C++
|
#include "pid.h"
|
||
|
|
#include "board.h"
|
||
|
|
#include "print.h"
|
||
|
|
|
||
|
|
PID::PID(float *_input, float *_feedback, float _kp, float _ki, float _kd, unsigned short _max_output)
|
||
|
|
: kp(_kp), ki(_ki), kd(_kd), max_output(_max_output * 1.0), input(_input), feedback(_feedback)
|
||
|
|
{
|
||
|
|
Clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
void PID::Clear()
|
||
|
|
{
|
||
|
|
err = integra = derivative = preErr = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
void PID::Update(float _kp, float _ki, float _kd, unsigned short _maxOutput)
|
||
|
|
{
|
||
|
|
kp = _kp;
|
||
|
|
ki = _ki;
|
||
|
|
kd = _kd;
|
||
|
|
max_output = _maxOutput;
|
||
|
|
}
|
||
|
|
|
||
|
|
short PID::Compute(float interval)
|
||
|
|
{
|
||
|
|
// 计算输入值和反馈值的差值
|
||
|
|
err = *input - *feedback;
|
||
|
|
// 计算积分误差
|
||
|
|
integra = integra + err * interval;
|
||
|
|
// 计算微分参数
|
||
|
|
derivative = (err - preErr) / interval;
|
||
|
|
// 更新误差
|
||
|
|
preErr = err;
|
||
|
|
|
||
|
|
if (ki != 0)
|
||
|
|
{
|
||
|
|
#if PID_DEBUG_OUTPUT
|
||
|
|
log("integra=%ld max_output=%ld %ld", long(integra * 1000), long(-(max_output / ki * 1000)), long(max_output / ki * 1000));
|
||
|
|
#endif
|
||
|
|
if (integra < -(max_output / ki))
|
||
|
|
{
|
||
|
|
#if PID_DEBUG_OUTPUT
|
||
|
|
log("integra clear-");
|
||
|
|
#endif
|
||
|
|
integra = -(max_output / ki);
|
||
|
|
}
|
||
|
|
if (integra > max_output / ki)
|
||
|
|
{
|
||
|
|
#if PID_DEBUG_OUTPUT
|
||
|
|
log("integra clear+");
|
||
|
|
#endif
|
||
|
|
integra = max_output / ki;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// 计算PID值
|
||
|
|
float val = err * kp + integra * ki + derivative * kd;
|
||
|
|
|
||
|
|
// 输出范围约束
|
||
|
|
if (val < -max_output)
|
||
|
|
val = -max_output + 1;
|
||
|
|
else if (val > max_output)
|
||
|
|
val = max_output - 1;
|
||
|
|
|
||
|
|
#if PID_DEBUG_OUTPUT
|
||
|
|
log("error=%ld integra=%ld derivative=%ld val=%ld", long(error * 1000), long(integra * 1000), long(derivative * 1000), long(val * 1000));
|
||
|
|
#endif
|
||
|
|
|
||
|
|
return val;
|
||
|
|
}
|