PID运算无效，与C#模拟不同。是为何？（4轴）

feel

如题，小弟最近在做四轴。目前已经处于PID调节平衡的阶段。

开发软件：MDK5.0
硬件环境：Mini 开发板

直接上部分代码，完整代码请看附件。源代码都上传了，不过估计硬件不同你们也没法调试。所以还是请大神慢慢看代码哈。

定义2个PID运算函数，PID1控制X轴   PID2控制Y轴

u8 tkp=30,tki=0,tkd=10;

 double PID1(float e,float kp,float ki,float kd)

{

static float e_s=0,sum=0;//e_s用于保存上一次的误差值，用于计算微分项。Sum用于计算累加和，计算积分项。

double r;

sum+=e;//计算积分累加和

r=kp*e+ki*sum+kd*(e-e_s);//从左至右分别是比例项、积分项、微分项

e_s=e;//保存这一次的误差值用于下一次微分计算

return r;

}

double PID2(float e,float kp,float ki,float kd)

{

static float e_s1=0,sum1=0;//e_s用于保存上一次的误差值，用于计算微分项。Sum用于计算累加和，计算积分项。

double r1;

sum1+=e;//计算积分累加和

r1=kp*e+ki*sum1+kd*(e-e_s1);//从左至右分别是比例项、积分项、微分项

e_s1=e;//保存这一次的误差值用于下一次微分计算

return r1;

}

/*控制PID 与控制值的映射范围。short取值范围32767  -32768

电调24-128  但是不能全速控制，全速估计会失控。暂定一部分用于PID控制。

正在运行的时候必须把PID计算也移入是否运行状态，否则多浪费CPU
 X:angle[0]    Y: angle[1]
分别对应4个电机   X+ X-  Y+ Y-
X范围     -180-----180  Y范围 -180-------180
6轴陀螺仪是淘宝买的直接串口输出那个，PID代码也是那店给的。
直接把X 和Y 丢进去运算。
判断返回的值，如果是+角度把增益给 X- 电机
如果是-  把增益给X+
这样来达到控制平衡的目的。

*/

controls[4]=(short)PID1(angle[0],tkp,tki,tkd)/256.0;

//单通道输出，输出完毕PID增益复位

if(controls[4]<0){

    controls[1]=(short)fabs(controls[4]);

}else{

    controls[1]=0;

}

if(controls[4]>0){

    controls[0]=controls[4];

else{

    controls[0]=0;

}

controls[5]=(short)PID2(angle[1],tkp,tki,tkd)/256.0;

if(controls[5]<0){

    controls[3]=(short)fabs(controls[5]);

}else{

    controls[3]=0;

}

if(controls[5]<0){

    controls[2]=controls[5];

}else{

    controls[2]=0;

}

printf("Xj:%.2f  Yj:%.2f  Zj:%.2f  Xs:%.2f  Ys:%.2f  Zs:%.2f\r\n",angle[0],angle[1],angle[2],w[0],w[1],w[2]);

printf("1Z:%.2f 2Z:%.2f 3Z:%.2f 4Z:%.2f \r\n",controls[0],controls[1],controls[2],controls[3]);

串口调试助手返回值

Xj:-6.27  Yj:-3.74  Zj:-15.85  Xs:13.79  Ys:-14.95  Zs:-1.46

1Z:0.00 2Z:-3.74 3Z:-15.85 4Z:13.79 

Xj:-5.58  Yj:-5.33  Zj:-15.86  Xs:0.06  Ys:-0.12  Zs:0.06

1Z:0.00 2Z:-5.33 3Z:-15.86 4Z:0.06 

Xj:-5.58  Yj:-5.38  Zj:-15.83  Xs:0.12  Ys:-0.06  Zs:0.06

1Z:0.00 2Z:-5.38 3Z:-15.83 4Z:0.12 

Xj:-5.59  Yj:-5.44  Zj:-15.81  Xs:-0.06  Ys:0.00  Zs:0.00

1Z:0.00 2Z:-5.44 3Z:-15.81 4Z:-0.06 

调整角度   XJ YJ  这2个角度测量是有变化的。
1Z 2Z 3Z 4Z 都有变化
但是1Z总是为0   3Z 4Z同步输出


C#控制程序的模拟PID

        void pidtest(int e, int kp, int ki, int kd)

        {

            float e_s = 0, sum = 0;//e_s用于保存上一次的误差值，用于计算微分项。Sum用于计算累加和，计算积分项。

            double r;

            sum += e;//计算积分累加和

            r = kp * e + ki * sum + kd * (e - e_s);//从左至右分别是比例项、积分项、微分项

            e_s = e;//保存这一次的误差值用于下一次微分计算

            r = r / 256;

            lbl_PID_OUT.Text = r.ToString();

        }

图中 按照这个PID值计算 180° P:30 I:0 D:0
输出的补偿增益不过是28.125
而与串口调试助手大不相同。。。。这是为何  为过来的数据还有负数 明明用了FABS函数 绝对值啊。。

求大神支招，小弟已经研究了3天了。基础差无奈 估计高不下去了。希望我的4轴能快点飞啊。

正点原子

帮顶