|
|
发表于 2011-1-24 21:22:09
|
显示全部楼层
贴一个我这两天写的离散PID模块吧，已经成功的用于舵机控制。PID是最简单的控制，无法掌握规律的时候用不错。
这个是在ucos里用的，如果裸机跑的话，把FP32换成float。
水平有限，望过路的同学指正。
PID的运作原理是每个控制周期，获取反馈模块提供的当前值，与上级模块提供的目标值进行比较，得到误差值，同时移位更新误差历史。然后output = A*e0 - B*e1 + C*e2就能得到控制输出值。这个控制输出值在不同的上下文中有不同的含义，如果是控制电机，这个控制值可以是转速。
简单的例子就是利用三个位置误差历史，通过运算得到电机的转速，以实现准确定位。
具体的解释这里有一篇论文。
点击此处下载 ourdev_613203OBSZGV.pdf(文件大小:1.30M) (原文件名:基于增量式PID控制实现高精度跟踪机构设计研究.pdf)
头文件：
#ifndef DISCRETE_PID_H_
#define DISCRETE_PID_H_
#include "includes.h"
/*Data Structure*/
typedef struct pid_parameterpack
{
FP32 fKp;
FP32 fKi;
FP32 fKd;
}PID_PP;
typedef struct pid_runtimepack
{
FP32 fA;
FP32 fB;
FP32 fC;
FP32 fError[3];
}PID_RP;
typedef struct pid_module
{
PID_PP cPP;
PID_RP cRP;
}PID_MODULE;
void PID_Reset(PID_MODULE *pPID,FP32 fTargetVal, FP32 fCurrentVal);
FP32 PID_CalculateControlOutput(PID_MODULE *pPID,FP32 fTargetVal, FP32 fCurrentVal);
#endif /* DISCRETE_PID_H_ */
源文件：
void PID_Reset(PID_MODULE *pPID,FP32 fTargetVal, FP32 fCurrentVal)
{
//calculate factors
pPID->cRP.fA = pPID->cPP.fKp + pPID->cPP.fKi + pPID->cPP.fKd;
pPID->cRP.fB = pPID->cPP.fKp + 2*pPID->cPP.fKd;
pPID->cRP.fC = pPID->cPP.fKd;
//set 3 errors to current error
pPID->cRP.fError[0] = fCurrentVal - fTargetVal;
pPID->cRP.fError[1] = pPID->cRP.fError[0];
pPID->cRP.fError[2] = pPID->cRP.fError[0];
}
FP32 PID_CalculateControlOutput(PID_MODULE *pPID,FP32 fTargetVal, FP32 fCurrentVal)
{
FP32 fPIDVal;
//update errors 0->1->2
pPID->cRP.fError[2] = pPID->cRP.fError[1];
pPID->cRP.fError[1] = pPID->cRP.fError[0];
pPID->cRP.fError[0] = fCurrentVal - fTargetVal;
//calculate pid value
fPIDVal = pPID->cRP.fError[0] * pPID->cRP.fA
- pPID->cRP.fError[1] * pPID->cRP.fB
+ pPID->cRP.fError[2] * pPID->cRP.fC;
return (fPIDVal);
} |
|
