位置式PID算法

最新推荐文章于 2024-09-06 18:26:18 发布

cxs0913

最新推荐文章于 2024-09-06 18:26:18 发布

阅读量1.2k

点赞数

分类专栏： C语言 BLDC 文章标签：位置式PID C语言

本文链接：https://blog.csdn.net/cxs0913/article/details/103320888

版权

C语言同时被 2 个专栏收录

11 篇文章 1 订阅

订阅专栏

BLDC

4 篇文章 1 订阅

订阅专栏

1、
typedef struct PID
{
float kp,ki,kd,PreErr,Pout,Iout,Dout,I_sum,Dt,Imax;
}PID;

//===============================================================================
//姿态环角度环
//input: pid—pid指针，EurDesir—期望的欧拉角，measure—测量的欧拉角,
// output—输出期望的角速度
//===============================================================================
void PID_EurLoop(PID* pid,float EurDesir,float measure,float* Output)
{
float Err,Diff;

Err = EurDesir-measure; //得到误差
Diff = (Err-pid->PreErr)/pid->Dt;
pid->PreErr = Err; //更新前次误差

pid->Pout = pid->kp * Err;         //比例计算,P*误差
pid->Dout = pid->kd * Diff;

*Output = pid->Pout + pid->Iout + pid->Dout;

}

//===============================================================================
//角速度环
//
//===============================================================================
void PID_GRYLoop(PID* pid,float EurDesir,float measure,float* Output)
{
float Err,Diff;

Err = EurDesir-measure; //得到误差
Diff = (Err-pid->PreErr)/pid->Dt;
pid->PreErr=Err; //更新前次误差

if(fabs(Err)>=0.5 && fabs(Err)<=800  ) pid->I_sum+=(Err*pid->Dt);       
if (pid->I_sum>  pid->Imax  )        pid->I_sum =    pid->Imax;
if (pid->I_sum<-(pid->Imax) )        pid->I_sum =   -(pid->Imax);
if (fly_state == FlyStop) pid->I_sum=0;

pid->Pout = pid->kp * Err;          //比例计算,P*误差
pid->Iout = pid->ki * pid->I_sum;
pid->Dout = pid->kd * Diff;

*Output=pid->Pout+pid->Iout+pid->Dout;

}