#include <string.h>
#include<stdio.h>
typedef struct PID{
double Command; //输入指令
double Proportion; //比例系数
double Integral; //积分系数
double Derivative; //微分系数
double preErr; //前一拍误差
double sumErr; //误差累积
}PID;
double PIDCale(PID *p,double feedback)
{
double dErr,Err;
Err=p->Command-feedback; //当前误差
p->sumErr+=Err; //误差累加
dErr=Err-p->preErr; //误差微分
p->preErr=Err;
return(p->Proportion*Err //比例项
+p->Derivative*dErr //微分项
+p->Integral*p->sumErr); //积分项
}
void PIDInit(PID *p)
{
memset(p,0,sizeof(PID)); //初始化
}
typedef struct motor{
double lastY;
double preY;
double lastU;
double preU;
}motor;
void motorInit(motor *m)
{
memset(m,0,sizeof(motor));//将数组 m[sizeof(motor)]清零
}
double motorCal(motor *m,double u)
{
double