互补滤波算法的了解大约如下。陀螺仪的短期测得的值比照准，可是因为存在温漂，进过积分，长时刻就不能用了。所以要经过加快度计进行批改视点，可是加快度计的短期不可，噪声比照大，所以要将陀螺仪高通，加快度计低通后进行必定比列的交融。才干比照精确的反响视点的改动。
我是这么做的：陀螺仪和加快度计别离进行了递推和一阶滤波，得到的滤波作用再按必定的份额进行拟合，采样周期为0.3ms。可是出来的作用不抱负。究竟直立操控量=P*（交融滤波后的视点）+D*(角速度（即陀螺仪的值）)
angle=(int)(0.99*(angle+(gyro-2940)*0.008)+0.01*(y_acc-3150))
int PIDCalc( unsigned int NextPoint )
{
int PID;
Error = standard - NextPoint; // 过失E(t)
iError+=Error; //E(t)+E(t-1)+...+E(1)
dError=Error-LastError; //E(t)-E(t-1)
PID=(int)(Proportion * Error //份额
+Integral*iError //积分
+ Derivative * dError); // 微分项
PrevError = LastError;
LastError = Error;
return (
PID
);
}
void PIDBEGIN()
{
Proportion = 6; // Set PID Coefficients 10
Integral=0.002;
Derivative = 120; // 4
}
传感器是mma-7361,enc-03

上一篇：单片机晶振电路计划阐明

下一篇：继电器自锁电路图原理