用c语言写一个adrc的代码

ADRC（Active Disturbance Rejection Control）算法是一种强大的控制方法，可以有效地处理系统的不确定性和外部干扰。下面是一个用C语言实现ADRC控制器的示例代码：

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define N 1000      // 控制器采样次数
#define h 0.01      // 采样周期

// 定义控制器参数结构体
typedef struct
{
    double h;           // 采样周期
    double alpha;       // 系数
    double e1;          // 系数
    double e2;          // 系数
    double r;           // 系数
    double u;           // 控制器输出
    double u_1;         // 上一时刻控制器输出
    double e;           // 控制误差
    double e_1;         // 上一时刻控制误差
    double e_2;         // 上上一时刻控制误差
    double d;           // 系统干扰
} ADRC;

// 初始化ADRC控制器参数
void initADRC(ADRC *adrc, double alpha, double e1, double e2, double r, double u)
{
    adrc->alpha = alpha;
    adrc->e1 = e1;
    adrc->e2 = e2;
    adrc->r = r;
    adrc->u = u;
    adrc->u_1 = 0.0;
    adrc->e = 0.0;
    adrc->e_1 = 0.0;
    adrc->e_2 = 0.0;
    adrc->d = 0.0;
}

// 计算ADRC控制器输出
double calcADRC(ADRC *adrc, double y, double r)
{
    // 计算系统干扰
    adrc->d = y - r;

    // 计算控制误差
    adrc->e = y - adrc->u;

    // 计算上一时刻控制误差
    adrc->e_1 = adrc->e;

    // 计算上上一时刻控制误差
    adrc->e_2 = adrc->e_1;

    // 计算控制器输出
    adrc->u = adrc->u_1 + adrc->h * (adrc->e + adrc->alpha * (adrc->e - adrc->e_1) + adrc->e1 * adrc->d - adrc->e2 * adrc->e_2);

    // 更新上一时刻控制器输出
    adrc->u_1 = adrc->u;

    return adrc->u;
}

int main()
{
    ADRC adrc;
    double y, r;
    int i;

    // 初始化控制器参数
    initADRC(&adrc, 0.5, 1.0, 0.5, 1.0, 0.0);

    // 模拟控制器过程
    for (i = 0; i < N; i++)
    {
        // 读取系统反馈信号和参考信号
        y = sin(i * h);
        r = 0.5 * sin(i * h);

        // 计算控制器输出
        adrc.u = calcADRC(&adrc, y, r);

        // 输出控制器输出结果
        printf("u[%d] = %f\n", i, adrc.u);
    }

    return 0;
}

在该示例代码中，我们定义了一个ADRC控制器参数结构体`ADRC`，包含了控制器的各个参数和状态变量。在`initADRC`函数中，我们初始化了控制器参数；在`calcADRC`函数中，我们计算了控制器的输出，并更新了控制器的状态变量。在`main`函数中，我们模拟了控制器的过程，并输出了控制器的输出结果。