PID算法验证
2024-10-06 08:54:25
算法：
struct PID {
float kp;
float kpnfac;
float ki;
float kinfac;
float kd;
};
float gCurPPM = 1300;
float getCurppm() { return gCurPPM; }
float getOut(PID& pid, float dppm)
{
static float disum = 0;
static float diffLast = 0;
float cppm = getCurppm();
float diff = dppm-cppm;
float dp = diff*pid.kp;
float di = diff*pid.ki;
if (diff<0) {
dp *= pid.kpnfac;
di *= pid.kinfac;
}
disum += di;
float out = dp + disum + (diff-diffLast)*pid.kd;
diffLast = diff;
printf("Cur=%4.0f dest=%4.0f, sumi=%4.1f out=%4.0f\n", cppm, dppm, disum, out);
return out;
}
验证代码：
#define DEFAULT_PTERM 0.25f
#define DEFAULT_PNFAC 10.0f
#define DEFAULT_ITERM 0.0006f
#define DEFAULT_INFAC 0.15f
int main()
{
PID pid;
pid.kp = DEFAULT_PTERM;
pid.kpnfac = DEFAULT_PNFAC;
pid.ki = DEFAULT_ITERM;
pid.kinfac = DEFAULT_INFAC;
pid.kd = -0.001;
for(int i=0;i <1000; i++){
float out = getOut(pid, 1900);
gCurPPM += out*2/3;
}
return 0;
}
验证输出：
Cur=1300 dest=1900, sumi= 0.4 out= 150
Cur=1400 dest=1900, sumi= 0.7 out= 126
Cur=1484 dest=1900, sumi= 0.9 out= 105
Cur=1554 dest=1900, sumi= 1.1 out= 88
Cur=1612 dest=1900, sumi= 1.3 out= 73
Cur=1661 dest=1900, sumi= 1.4 out= 61
Cur=1702 dest=1900, sumi= 1.6 out= 51
Cur=1736 dest=1900, sumi= 1.7 out= 43
Cur=1764 dest=1900, sumi= 1.7 out= 36
Cur=1788 dest=1900, sumi= 1.8 out= 30
Cur=1808 dest=1900, sumi= 1.9 out= 25
Cur=1825 dest=1900, sumi= 1.9 out= 21
Cur=1838 dest=1900, sumi= 1.9 out= 17
Cur=1850 dest=1900, sumi= 2.0 out= 14
Cur=1860 dest=1900, sumi= 2.0 out= 12
Cur=1868 dest=1900, sumi= 2.0 out= 10
Cur=1874 dest=1900, sumi= 2.0 out= 8
Cur=1880 dest=1900, sumi= 2.0 out= 7
Cur=1885 dest=1900, sumi= 2.0 out= 6
Cur=1889 dest=1900, sumi= 2.1 out= 5
Cur=1892 dest=1900, sumi= 2.1 out= 4
Cur=1895 dest=1900, sumi= 2.1 out= 3
Cur=1897 dest=1900, sumi= 2.1 out= 3
Cur=1899 dest=1900, sumi= 2.1 out= 2
Cur=1900 dest=1900, sumi= 2.1 out= 1
Cur=1901 dest=1900, sumi= 2.1 out= -1
Cur=1901 dest=1900, sumi= 2.1 out= 0
Cur=1901 dest=1900, sumi= 2.1 out= -0
Cur=1901 dest=1900, sumi= 2.1 out= 0
Cur=1901 dest=1900, sumi= 2.1 out= -0
Cur=1901 dest=1900, sumi= 2.1 out= 0
Cur=1901 dest=1900, sumi= 2.1 out= -0
Cur=1901 dest=1900, sumi= 2.1 out= 0
Cur=1901 dest=1900, sumi= 2.1 out= -0
Cur=1901 dest=1900, sumi= 2.1 out= 0
Cur=1901 dest=1900, sumi= 2.1 out= -0
Cur=1901 dest=1900, sumi= 2.1 out= 0
Cur=1901 dest=1900, sumi= 2.1 out= -0
最新文章
- 验证控件jQuery Validation Engine简单自定义正则表达式
- 【初级】linux pwd 命令详解及使用方法实战
- javascript-简单工厂两种实现方式
- Sql Server2005恢复备份数据库问题－Error:3154 3219
- php提示:Call to undefined function curl_init
- NGUI Font
- 2016 年开发者应该掌握的十个 Postgres 技巧
- jqGrid源代码分析（一）
- css 元素居中方法
- 该用Python还是SQL？4个案例教你节省时间
- RPA基础
- Shell脚本中的 测试开关 和 特殊参数
- RGMII_PHY测试笔记1 基于开发板MiS603-X25
- Zookeeper--0300--java操作Zookeeper，临时节点实现分布式锁原理
- 20155218 Exp1 PC平台逆向破解(5)M
- LAMP平台-wordpress的搭建
- CSDN无耻，亿赛通无耻
- Chrome 的应用功能越来越强大
- 洛谷 P3802 小魔女帕琪 解题报告
- [Android Pro] 有关Broadcast作为内部类时注册的一些问题