98 lines
2.9 KiB
C
98 lines
2.9 KiB
C
#include "pid.h"
|
|
#include <math.h>
|
|
// 设置控制参数
|
|
static void _set_ctrl_prm(struct PID_NEURAL *self, float32 minimum, float32 maximum)
|
|
{
|
|
self->pri.setpoint = minimum; /*设定值*/
|
|
|
|
self->pri.kcoef = 0.12; /*神经元输出比例*/
|
|
self->pri.kp = 0.45; /*比例学习速度*/
|
|
self->pri.ki = 0.05; /*积分学习速度*/
|
|
self->pri.kd = 0; /*微分学习速度*/
|
|
|
|
self->pri.lasterror = 0.0; /*前一拍偏差*/
|
|
self->pri.preerror = 0.0; /*前两拍偏差*/
|
|
self->pri.result = minimum; /*PID控制器结果*/
|
|
self->pri.output = 0.0; /*输出值，百分比*/
|
|
|
|
self->pri.maximum = maximum; /*输出值上限*/
|
|
self->pri.minimum = minimum; /*输出值下限*/
|
|
self->pri.deadband = (maximum - minimum) * 0.0005f; /*死区*/
|
|
|
|
self->pri.wp = 0.10; /*比例加权系数*/
|
|
self->pri.wi = 0.10; /*积分加权系数*/
|
|
self->pri.wd = 0.10; /*微分加权系数*/
|
|
}
|
|
|
|
// 设置输出参数
|
|
static void _set_out_prm(struct PID_NEURAL *self, float32 minimum, float32 maximum)
|
|
{
|
|
self->pri.maximum = maximum;
|
|
self->pri.minimum = minimum;
|
|
}
|
|
|
|
/*单神经元学习规则函数*/
|
|
static void NeureLearningRules(struct PID_NEURAL *self, float32 zk, float32 uk, float32 *xi)
|
|
{
|
|
self->pri.wi = self->pri.wi + self->pri.ki * zk * uk * xi[0];
|
|
self->pri.wp = self->pri.wp + self->pri.kp * zk * uk * xi[1];
|
|
self->pri.wd = self->pri.wd + self->pri.kd * zk * uk * xi[2];
|
|
}
|
|
|
|
static float32 _PID(struct PID_NEURAL *self, float32 target, float32 feedback)
|
|
{
|
|
float32 x[3];
|
|
float32 w[3];
|
|
float32 sabs;
|
|
float32 error;
|
|
float32 result;
|
|
float32 deltaResult;
|
|
self->pri.setpoint = target;
|
|
error = self->pri.setpoint - feedback;
|
|
result = self->pri.result;
|
|
if (fabs(error) > self->pri.deadband)
|
|
{
|
|
x[0] = error;
|
|
x[1] = error - self->pri.lasterror;
|
|
x[2] = error - self->pri.lasterror * 2 + self->pri.preerror;
|
|
|
|
sabs = fabs(self->pri.wi) + fabs(self->pri.wp) + fabs(self->pri.wd);
|
|
w[0] = self->pri.wi / sabs;
|
|
w[1] = self->pri.wp / sabs;
|
|
w[2] = self->pri.wd / sabs;
|
|
|
|
deltaResult = (w[0] * x[0] + w[1] * x[1] + w[2] * x[2]) * self->pri.kcoef;
|
|
}
|
|
else
|
|
{
|
|
deltaResult = 0;
|
|
}
|
|
|
|
result = result + deltaResult;
|
|
if (result > self->pri.maximum)
|
|
{
|
|
result = self->pri.maximum;
|
|
}
|
|
if (result < self->pri.minimum)
|
|
{
|
|
result = self->pri.minimum;
|
|
}
|
|
self->pri.result = result;
|
|
self->pri.output = self->pri.result;
|
|
|
|
// 单神经元学习
|
|
NeureLearningRules(self, error, result, x);
|
|
|
|
self->pri.preerror = self->pri.lasterror;
|
|
self->pri.lasterror = error;
|
|
|
|
return self->pri.output;
|
|
}
|
|
|
|
void pid_neural_constructor(struct PID_NEURAL *self)
|
|
{
|
|
self->set_ctrl_prm = _set_ctrl_prm;
|
|
self->set_out_prm = _set_out_prm;
|
|
self->PID = _PID;
|
|
}
|