ladrc c代码

#include “ADRC.h”
//
//函数名：void ADRC_Init(ADRC_NUM *ADRC_TYPE1)
//函数说明：LADRC参数初始化
//
void ADRC_Init(ADRC_NUM *ADRC_TYPE1)
{
ADRC_TYPE1->h=0.005; //定时时间及时间步长
ADRC_TYPE1->r=100; //跟踪速度参数
ADRC_TYPE1->w0=50; //观测器带宽
ADRC_TYPE1->wc=10; //状态误差反馈率带宽
}

//
//函数名：void ADRC_TD(ADRC_NUM *ADRC_TYPE1,float Expect)
//函数说明：LADRC跟踪微分部分
//入口参数，期望值Expect(v0)输出值v1,v2
//
void ADRC_TD(ADRC_NUM ADRC_TYPE1,float Expect)
{
float fh= -ADRC_TYPE1->rADRC_TYPE1->r*(ADRC_TYPE1->v1-Expect)-2ADRC_TYPE1->rADRC_TYPE1->v2;
ADRC_TYPE1->v1+=ADRC_TYPE1->v2ADRC_TYPE1->h;
ADRC_TYPE1->v2+=fhADRC_TYPE1->h;
}

void ADRC_ESO(ADRC_NUM ADRC_TYPE1,float FeedBack)
{
float Beita_01=3ADRC_TYPE1->w0;
float Beita_02=3ADRC_TYPE1->w0ADRC_TYPE1->w0;
float Beita_03=ADRC_TYPE1->w0ADRC_TYPE1->w0ADRC_TYPE1->w0;

float e= ADRC_TYPE1->z1-FeedBack;
ADRC_TYPE1->z1+= (ADRC_TYPE1->z2 - Beita_01e)ADRC_TYPE1->h;
ADRC_TYPE1->z2+= (ADRC_TYPE1->z3 - Beita_02e + ADRC_TYPE1->b0ADRC_TYPE1->u)ADRC_TYPE1->h;
ADRC_TYPE1->z3+=-Beita_03e*ADRC_TYPE1->h;
}

void ADRC_LF(ADRC_NUM ADRC_TYPE1)
{
float Kp=ADRC_TYPE1->wcADRC_TYPE1->wc;
float Kd=3ADRC_TYPE1->wc;
float e1=ADRC_TYPE1->v1-ADRC_TYPE1->z1;
float e2=ADRC_TYPE1->v2-ADRC_TYPE1->z2;
float u0=Kpe1+Kd*e2;
ADRC_TYPE1->u=(u0-ADRC_TYPE1->z3)/ADRC_TYPE1->b0;
}

void ADRC_Loop(ADRC_NUM *ADRC_TYPE1)
{
ADRC_TD(ADRC_TYPE1,Expect_Value);
ADRC_ESO(ADRC_TYPE1,Measure);
ADRC_LF(ADRC_TYPE1);
}

#ifndef _ADRC_H
#define _ADRC_H
#include “stm32f10x.h”
typedef struct ADRC
{
float v1,v2; //¸最速输出值
float r; //速度因子
float h; //步长
float z1,z2,z3; //观测器输出
float w0,wc,b0,u; //
}ADRC_NUM;
extern ADRC_NUM ADRC_IN1,ADRC_IN2,ADRC_IN3;
void ADRC_Loop(ADRC_NUM *ADRC_TYPE1);
#endif