请写出一份ADRC的c语言代码
时间: 2023-06-01 22:02:32 浏览: 163
由于ADRC算法的实现方式有多种,因此这里提供一种常见的实现方式的C语言代码:
```
#include <stdio.h>
#define Ts 0.01 // 采样时间
#define h0 10 // 初始估计值
#define b0 20 // 初始估计值
#define a1 1.8 // 参数a1
#define a2 1.2 // 参数a2
#define a3 0.6 // 参数a3
#define L 100 // 滑动窗口长度
float ADRC(float y, float r)
{
static float h = h0; // 估计值
static float b = b0; // 平衡控制量
static float u = 0; // 控制量
static float e1 = 0; // 一阶误差
static float e2 = 0; // 二阶误差
static float e3 = 0; // 三阶误差
static float delta = 0; // 摩擦力补偿量
static float s[L] = {0}; // 滑动窗口
static int i = 0; // 滑动窗口指针
// 计算误差和误差变化率
float err = r - y;
float err_dot = (err - e1) / Ts;
float err_dot2 = (err_dot - e2) / Ts;
// 计算摩擦力补偿量
if (err_dot2 < 0) {
delta = -a1 * err_dot2;
} else {
delta = -a2 * err_dot2;
}
// 计算估计值和平衡控制量
h = h + Ts * b;
b = b + Ts * a3 * err_dot2;
// 计算控制量
u = h + b - delta;
// 更新误差和滑动窗口
e3 = e2;
e2 = e1;
e1 = err;
s[i++] = err;
if (i >= L) {
i = 0;
}
// 计算滑动平均误差
float s_avg = 0;
for (int j = 0; j < L; j++) {
s_avg += s[j];
}
s_avg /= L;
// 计算控制量修正量
float u_d = a3 * (s_avg - err_dot);
// 返回最终控制量
return u + u_d;
}
int main()
{
float y = 0; // 实际输出值
float r = 1; // 参考输入值
// 模拟实时控制过程
for (int i = 0; i < 1000; i++) {
float u = ADRC(y, r); // 计算控制量
y = /* 通过某种方式获取实际输出值 */;
/* 通过某种方式将控制量u应用到被控对象上 */;
}
return 0;
}
```
注意:以上代码仅供参考,实际应用中需要根据具体情况进行修改和优化。
阅读全文