匿名上位机调节PID成功，有视频，有源码 (amobbs.com 阿莫电子技术论坛) -

10xjzheng 发表于 2015-7-16 12:45:13

匿名上位机调节PID成功，有视频，有源码

花了两天，搞定了PID，匿名上位机帮了我的大忙！上位机可以直接修改PID的值，还可以观察角度、滤波之后的角度、PWM控制量等等的波形。
先上视频，现在PID控制得算是比较好了，从视频中可以看到我动了帆板一下，很快就恢复回原来的状态，开始调不好，出现了震荡，很久才恢复到初始角度。
http://v.youku.com/v_show/id_XMTI4NTk5NjQ0MA==.html
很多人可能更感兴趣匿名上位机，匿名上位机真的很强大！可以直接修改PID，显示波形。

匿名上位机跟单片机的通讯程序在帮助里面有

上位机跟通讯协议

PID程序如下。
/*************************************************

名称：void pid(float angle, float angle_dot)
功能：PID运算
输入参数：
float angle 倾斜角度
      float angle_dot 倾斜角速度
输出参数：无
返回值：无
**************************************************/
//增量式PID
int pid(PID_Type *PID,float angle, float angle_dot)
{
   static float err=0,err1=0,err2=0;//本次、上次、上上次误差
   float add;//增量

   //如果PID1改变了，将err都置0
   if(PID->SetPointChange==1)
   {
            PID->SetPointChange=0;
            err=err1=err2=0;
   }

   //计算误差
   err=PID->SetPoint-angle;
   //计算增量
   add=PID->kP*(err-err1)+PID->kI*err+PID->kD*(err-2*err1+err2);
   //加上增量
   PID->CtrlValue+=add;
   //限幅
   if(PID->CtrlValue>MAX_PWM)PID->CtrlValue=MAX_PWM;
   if(PID->CtrlValue<0)PID->CtrlValue=0;

   //保存误差
   err2=err1;
   err1=err;

   return PID->CtrlValue;
}

//位置式
int pid2(PID_Type *PID,float angle, float angle_dot)
{
   //err0 本次的误差，err1 累计的误差，err2 这次跟前次误差的差值 err3 上次的误差
   static float err0=0,err1=0,err2=0,err3=0;

   //如果PID1改变了，将err都置0
   if(PID->SetPointChange==1)
   {
            PID->SetPointChange=0;
            err0=err1=err2=0;
   }

   //计算误差
   err0=PID->SetPoint-angle;
   //误差累加
   err1+=err0;
   //计算误差差值
   err2=err0-err3;
   //保存这次的误差
   err3=err0;
   //计算控制量
   PID->CtrlValue=PID->kP*err0+PID->kI*err1+PID->kD*err2;

   //限幅
   if(PID->CtrlValue>MAX_PWM)PID->CtrlValue=MAX_PWM;
   if(PID->CtrlValue<0)PID->CtrlValue=0;

   return PID->CtrlValue;
}

10xjzheng 发表于 2015-7-16 17:45:18

lyg407 发表于 2015-7-16 17:09
楼主解释下这个是什么意思，那个是有刷电机？吹风然后检测板子角度板子上面一个加速度陀螺 ...

话说我以为你们懂的，直流电机吹板子会不稳定的，也不能到达一个角度。
所以我就将角度反馈回来，用PID计算出PWM控制电机的转速。

10xjzheng 发表于 2015-7-18 11:02:54

爱新觉罗_极刚霸发表于 2015-7-18 10:25
不知道51可以实现不？

串口速率够就行，不过估计占用51资源会比较多

10xjzheng 发表于 2015-7-18 11:32:12

顺便记录下调试PID心得：
在调整小的变化的时候，上面的PID是没有问题的，但是当出现较大的变化，并且比较久的时候，我发现整个系统会出现较长时间的恢复，这是由于积分项在捣乱，把偏差一直积分，后面P在减小PID输出的时候要纠正积分作用就很困难了，这个需要将程序修改如下！
#define MAX_ERR20
//积分分离法的位置式PID
//当误差比较大的时候去掉积分环节
int pid3(PID_Type *PID,float angle, float angle_dot)
{
//err0 本次的误差，err1 累计的误差，err2 这次跟前次误差的差值 err3 上次的误差
static float err0=0,err1=0,err2=0,err3=0;

//如果PID1改变了，将err都置0
if(PID->SetPointChange==1)
{
PID->SetPointChange=0;
err0=err1=err2=0;
}

//计算误差
err0=PID->SetPoint-angle;
//误差累加
err1+=err0;
//计算误差差值
err2=err0-err3;
//保存这次的误差
err3=err0;
if(fabs(err0)<MAX_ERR)
{
//计算控制量
PID->CtrlValue=PID->kP*err0+PID->kI*err1+PID->kD*err2;
}
else
{
//
PID->CtrlValue=PID->kP*err0+PID->kD*err2;
}

//限幅
if(PID->CtrlValue>MAX_PWM)PID->CtrlValue=MAX_PWM;
if(PID->CtrlValue<0)PID->CtrlValue=0;

return PID->CtrlValue;
}

10xjzheng 发表于 2015-7-18 11:44:45

看到同学有另外一个比较好的算法，在平常的PID后面加上一个随角度偏移的角度量——这是在PID中的控制常量，一般PID没有！这样子PID工作起来就很方便，只需要在一个角度附近进行调整，但是获得一个随角度偏移的角度量这个是比较繁琐的！但是想到可以利用一开始调整好的PID（这个时候没有控制常量），然后得出比较合适的PWM！最后将这组常量做成一个数组，作为一个随目标角度变化的控制常量，然后再调整另外的一组PID！

10xjzheng 发表于 2015-7-18 21:31:02

爱新觉罗_极刚霸发表于 2015-7-18 21:18
多谢回复~下午试了下，可以绘制出传感器的曲线。不知道可不可以在上位机上直接修改PID参数进行调 ...

上位机发送数据下来，然后你进行解析，提取出功能字，具体看协议，然后将数据段保存到对应PID变量，最后要返回一个cheaksum

苹果520 发表于 2015-7-16 15:07:43

点赞下！

bd7qw 发表于 2015-7-16 15:24:54

不错呀，

longhua 发表于 2015-7-16 15:37:20

我就看看

Hearthbeats 发表于 2015-7-16 15:49:05

不错，上位机发给我一下。、

mvip 发表于 2015-7-16 16:35:56

围观学习中

lyg407 发表于 2015-7-16 17:09:28

楼主解释下这个是什么意思，那个是有刷电机？吹风然后检测板子角度板子上面一个加速度陀螺仪模块么？

lrzxc 发表于 2015-7-16 17:25:41

这个看着还不错

lans0625 发表于 2015-7-16 18:43:58

好资料，谢谢分享。一直没时间玩，有机会要试试试。

huang518489 发表于 2015-7-16 19:00:16

貌似是某年的竞赛题

10xjzheng 发表于 2015-7-16 19:22:25

huang518489 发表于 2015-7-16 19:00
貌似是某年的竞赛题

11年国赛题目

阿豪博士 发表于 2015-7-16 22:12:27

MARK一下！PID两种计算方法！

soos 发表于 2015-7-16 22:47:38

高大上吗？

阿豪博士 发表于 2015-7-17 11:09:06

请教一下，如果是双极PID，就是可以调节电机正反转的，使输出的PID有负数值，楼上可以指点一下不？

peiyan 发表于 2015-7-17 11:12:06

谢谢分享，果断收藏

10xjzheng 发表于 2015-7-17 11:14:58

阿豪博士发表于 2015-7-17 11:09
请教一下，如果是双极PID，就是可以调节电机正反转的，使输出的PID有负数值，楼上可以指点一下不？ ...

把我程序的限幅去掉，然后判断如果是小于0就翻转，大于0就正转

qqfirer 发表于 2015-7-17 11:19:08

好的，似曾相识，原来国赛题目

阿豪博士 发表于 2015-7-17 11:21:45

好的！
领悟了。
另外请教一个问题，楼主十分可以将这个工程开源一下？
让我学习一下整体的控制思路？
另外这个有必要设定死区吗？

阿豪博士 发表于 2015-7-17 11:23:49

好的！
领悟了。
另外请教一个问题，楼主十分可以将这个工程开源一下？
让我学习一下整体的控制思路？
另外这个有必要设定死区吗？

10xjzheng 发表于 2015-7-17 11:38:26

阿豪博士发表于 2015-7-17 11:23
好的！
领悟了。
另外请教一个问题，楼主十分可以将这个工程开源一下？

你要哪部分程序我直接发上来，工程不可以。
死区就看你调的效果，我不知道

阿豪博士 发表于 2015-7-17 11:46:41

好的！
领悟了。
另外请教一个问题，楼主十分可以将这个工程开源一下？
让我学习一下整体的控制思路？
另外这个有必要设定死区吗？

爱新觉罗_极刚霸 发表于 2015-7-18 10:25:15

不知道51可以实现不？

爱新觉罗_极刚霸 发表于 2015-7-18 21:18:29

10xjzheng 发表于 2015-7-18 11:02
串口速率够就行，不过估计占用51资源会比较多

{:lol:} 多谢回复~下午试了下，可以绘制出传感器的曲线。不知道可不可以在上位机上直接修改PID参数进行调节，上位机上的帮助信息有一部分没有看明白~

爱新觉罗_极刚霸 发表于 2015-7-18 21:20:39

ruan18278816371 发表于 2015-7-18 21:43:51

最近也在做平衡的题目，帮顶。楼主感兴趣也可以看看http://www.amobbs.com/forum.php?mod=viewthread&tid=5627184&extra={:lol:}

爱新觉罗_极刚霸 发表于 2015-7-19 08:41:02

10xjzheng 发表于 2015-7-18 21:31
上位机发送数据下来，然后你进行解析，提取出功能字，具体看协议，然后将数据段保存到对应PID变量，最后 ...

发送PID数据帧1....
PID数据帧1发送成功！
发送PID数据帧2....
通信出错，请重试

再次请教一下，上位机向下发送时会发送几个数据帧？？？

void Data_Receive_Anl(u8 *data_buf,u8 num)
{
vs16 rc_value_temp;
u8 sum = 0;

for(u8 i=0;i<(num-1);i++)
sum += *(data_buf+i);
if(!(sum==*(data_buf+num-1))) return; //判断sum
if(!(*(data_buf)==0xAA && *(data_buf+1)==0xAF)) return; //判断帧头
/////////////////////////////////////////////////////////////////////////////////////
if(*(data_buf+2)==0X01)
{
if(*(data_buf+4)==0X01)
MPU6050_CalOff_Acc();
if(*(data_buf+4)==0X02)
MPU6050_CalOff_Gyr();
if(*(data_buf+4)==0X03)
{MPU6050_CalOff_Acc();MPU6050_CalOff_Gyr();}
// if(*(data_buf+4)==0X04)
// Cal_Compass();
// if(*(data_buf+4)==0X05)
// MS5611_CalOffset();
}
if(*(data_buf+2)==0X02)
{
if(*(data_buf+4)==0X01)
{
Send_PID1 = 1;
Send_PID2 = 1;
Send_PID3 = 1;
Send_PID4 = 1;
Send_PID5 = 1;
Send_PID6 = 1;
}
if(*(data_buf+4)==0X02)
Send_Offset = 1;
}
#ifndef CONTROL_USE_RC
if(*(data_buf+2)==0X03)
{
Rc_D.THROTTLE = (vs16)(*(data_buf+4)<<8)|*(data_buf+5);
Rc_D.YAW = (vs16)(*(data_buf+6)<<8)|*(data_buf+7);
Rc_D.ROLL = (vs16)(*(data_buf+8)<<8)|*(data_buf+9);
Rc_D.PITCH = (vs16)(*(data_buf+10)<<8)|*(data_buf+11);
Rc_D.AUX1 = (vs16)(*(data_buf+12)<<8)|*(data_buf+13);
Rc_D.AUX2 = (vs16)(*(data_buf+14)<<8)|*(data_buf+15);
Rc_D.AUX3 = (vs16)(*(data_buf+16)<<8)|*(data_buf+17);
Rc_D.AUX4 = (vs16)(*(data_buf+18)<<8)|*(data_buf+19);
Rc_D.AUX5 = (vs16)(*(data_buf+20)<<8)|*(data_buf+21);
Rc_D.AUX6 = (vs16)(*(data_buf+21)<<8)|*(data_buf+22);
Rc_Fun(&Rc_D,&Rc_C);
}
#endif
if(*(data_buf+2)==0X10) //PID1
{
PID_ROL.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_ROL.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/1000;
PID_ROL.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_PIT.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_PIT.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/1000;
PID_PIT.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
PID_YAW.P = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17))/100;
PID_YAW.I = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19))/100;
PID_YAW.D = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21))/100;
Data_Send_Check(sum);
}
if(*(data_buf+2)==0X11) //PID2
{
PID_ALT.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_ALT.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/100;
PID_ALT.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_POS.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_POS.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/100;
PID_POS.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
PID_PID_1.P = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17))/100;
PID_PID_1.I = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19))/100;
PID_PID_1.D = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21))/100;
Data_Send_Check(sum);
}
if(*(data_buf+2)==0X12) //PID3
{
PID_PID_2.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_PID_2.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/100;
PID_PID_2.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_PID_3.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_PID_3.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/100;
PID_PID_3.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
PID_PID_4.P = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17))/100;
PID_PID_4.I = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19))/100;
PID_PID_4.D = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21))/100;
Data_Send_Check(sum);
}
if(*(data_buf+2)==0X13) //PID4
{
PID_PID_5.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_PID_5.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/100;
PID_PID_5.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_PID_6.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_PID_6.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/100;
PID_PID_6.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
PID_PID_7.P = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17))/100;
PID_PID_7.I = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19))/100;
PID_PID_7.D = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21))/100;
Data_Send_Check(sum);
}
if(*(data_buf+2)==0X14) //PID5
{
PID_PID_8.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_PID_8.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/100;
PID_PID_8.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_PID_9.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_PID_9.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/100;
PID_PID_9.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
PID_PID_10.P = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17))/100;
PID_PID_10.I = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19))/100;
PID_PID_10.D = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21))/100;
Data_Send_Check(sum);
}
if(*(data_buf+2)==0X15) //PID6
{
PID_PID_11.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_PID_11.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/100;
PID_PID_11.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_PID_12.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_PID_12.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/100;
PID_PID_12.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
Data_Send_Check(sum);
EE_SAVE_PID();
}
if(*(data_buf+2)==0X16) //OFFSET
{
AngleOffset_Rol = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/1000;
AngleOffset_Pit = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/1000;
}
}

这几个数据帧都会发送下来吗？虽然没有用到？

爱新觉罗_极刚霸 发表于 2015-7-19 08:49:55

10xjzheng 发表于 2015-7-19 10:29:02

爱新觉罗_极刚霸发表于 2015-7-19 08:41
发送PID数据帧1....
PID数据帧1发送成功！
发送PID数据帧2....

你在界面上看到的所有数据都会发送下来，你应该是少了检测了几个功能字，然后就少发送了cheaksum

10xjzheng 发表于 2015-7-19 10:29:22

具体看协议，excel文档即可知道！

爱新觉罗_极刚霸 发表于 2015-7-19 10:37:01

10xjzheng 发表于 2015-7-19 10:29
具体看协议，excel文档即可知道！

多谢你啦{:lol:} ，已经可以发送了~但因为收发都是在串口中断中处理的，所以有时上位机会接受失败或者上位机发送失败，不知道你是如何处理哒？
我用的是STC12C5A60S2

10xjzheng 发表于 2015-7-19 10:44:05

爱新觉罗_极刚霸发表于 2015-7-19 10:37
多谢你啦，已经可以发送了~但因为收发都是在串口中断中处理的，所以有时上位机会接受失败或者上 ...

我用的是状态机分析协议，搞到一帧数据，然后将获得的数据放到1缓冲区，接着在main中中进行处理缓冲区1，在给出一个缓冲区B在中断中进行接收，中断里面只是接收数据+几个判断。
void USART1_IRQHandler(void)
{
char com_data;

//如果是串口接收中断
if( USART_GetITStatus(USART1,USART_IT_RXNE) == SET)
{
USART_GetITStatus(USART1,USART_IT_RXNE);
com_data = USART_ReceiveData(USART1);
if(Rx_Adr==0) //寻找帧头0XAAAF
{
if(com_data==0xAA)
{
Rx_Buf = com_data;
Rx_Adr = 1;
}
}
else if(Rx_Adr==1)
{
if(com_data==0xAF)
{
Rx_Buf = com_data;
Rx_Adr = 2;
}
else
Rx_Adr = 0;
}
else if(Rx_Adr==2) //FUN
{
Rx_Buf = com_data;
Rx_Adr = 3;
}
else if(Rx_Adr==3) //LEN
{
Rx_Buf = com_data;
Rx_Adr = 4;
}
else
{
Rx_Buf = com_data;
Rx_Adr ++;
}

if(Rx_Adr==Rx_Buf+5) //数据接收完毕
{
Rx_Adr = 0;
if(Rx_Act)
{
Rx_Act = 0; //切换缓存
Rx_Ok1 = 1;
}
else
{
Rx_Act = 1;
Rx_Ok0 = 1;
}
}
}
}
你也可以用FIFO，在中断中放入缓冲区，然后while中取出来分析，应该可以防止通讯失败，但是效率应该没有上面的状态机高。

爱新觉罗_极刚霸 发表于 2015-7-19 11:18:09

10xjzheng 发表于 2015-7-19 10:44
我用的是状态机分析协议，搞到一帧数据，然后将获得的数据放到1缓冲区，接着在main中中进行处理缓冲区1， ...

你们也是电赛培训吧，感觉你好厉害啊！

xurenhui 发表于 2015-7-19 11:31:58

谢谢分享

阿豪博士 发表于 2015-7-20 08:11:52

10xjzheng 发表于 2015-7-17 11:38
你要哪部分程序我直接发上来，工程不可以。
死区就看你调的效果，我不知道 ...

好的楼主！
谢谢哦！
那我看看楼主对于PID计算中与定时器相关联的部分吧！
以及PID部分的定义结构体。
另外，楼主请指点一下，如果做一个正反方向转动的风机，并且可以两面吹风，双向控制角度这样的PID可以做成不？

10xjzheng 发表于 2015-7-20 09:15:24

阿豪博士发表于 2015-7-20 08:11
好的楼主！
谢谢哦！
那我看看楼主对于PID计算中与定时器相关联的部分吧！

/* 定义PID类型 */
typedef struct PID_Type PID_Type;

struct PID_Type
{
float SetPoint;
unsigned char SetPointChange;//设置目标发生变化
int CtrlValue;//0~1000
float kP;
float kI;
float kD;
};
下面的计算工程的计算频率时100Hz
MPU6050ReadAcc(Accel);
MPU6050ReadGyro(Gyro);

   //对加速度进行平滑滤波
acc_filter(Accel,Mag);
//将获得的原始数据转化为度/s
AnglePot=Gyro*GYRO_SCALE;
//根据滤波后的加速度计算角度
Angle=atan2(Accel,Accel)*57.295780;
//进行卡尔曼滤波
kalman_filter(Angle,AnglePot,&f_Angle,&f_AnglePot);
//对角度进行平滑滤波
//angle_filter(&PID1,Angle,&AfterAccel);
//返回PID的控制量，并设置占空比
SET_DUTY(speed_filter(pid2(&PID1,f_Angle,f_AnglePot)));

//显示波形
Mag=PID1.SetPoint;
Mag=Angle;
Mag=Duty;
//角度、角速度放到PID2里面去
PID2.kP=Angle;
PID2.kI=AnglePot;
PID2.kD=f_Angle;
//显示相差的大小
PID3.kD=fabs(PID1.SetPoint-Angle);
//波形数据从串口发送出去->蓝牙->空气->蓝牙->串口->串口转USB->电脑接收并处理
NingmingSend_Data(Gyro,Accel,Mag);
Task_Delay=10;
计算相关的函数
#include "include.h"
//卡尔曼滤波
static float angle, angle_dot;
const float Q_angle = 0.002, Q_gyro = 0.002, R_angle = 0.5, dt = 0.01;
static float P={
      { 1, 0 },
            { 0, 1 }
         };
static float Pdot = {0, 0, 0, 0};
const u8 C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

//平滑滤波
short ax_buf={0};
short ay_buf={0};
short az_buf={0};
short Angle_buf={0};

s16 speed_buf={0};
/*************************************************

名称：void acc_filter(void)
功能：加速度计数据滤波
输入参数：据滤波后的数据
输出参数：无
返回值：无
**************************************************/
void acc_filter(short *Accel,short *Mag)
{
u8 i;
s32 ax_sum = 0, ay_sum = 0, az_sum = 0;

for(i = 1 ; i < FILTER_COUNT; i++)
{
ax_buf = ax_buf;
   ay_buf = ay_buf;
   az_buf = az_buf;
}

ax_buf = Accel;
ay_buf = Accel;
az_buf = Accel;

for(i = 0 ; i < FILTER_COUNT; i++)
{
ax_sum += ax_buf;
   ay_sum += ay_buf;
   az_sum += az_buf;
}

Mag = (s16)(ax_sum / FILTER_COUNT);
Mag = (s16)(ay_sum / FILTER_COUNT);
Mag = (s16)(az_sum / FILTER_COUNT);
}

//角度的平滑滤波
void angle_filter(short Angle,short *AfterAngle)
{
u8 i;
s32 angle_sum = 0;

for(i = 1 ; i < FILTER_COUNT; i++)
{
Angle_buf = Angle_buf;
}

Angle_buf = Angle;

for(i = 0 ; i < FILTER_COUNT; i++)
{
angle_sum += Angle_buf;
}

(*AfterAngle)=(s16)(angle_sum / FILTER_COUNT);
}

/*************************************************

名称：void kalman_filter(float angle_m, float gyro_m, float *angle_f, float *angle_dot_f)
功能：陀螺仪数据与加速度计数据通过滤波算法融合
输入参数：
float angle_m 加速度计计算的角度
   float gyro_m 陀螺仪角速度

   float *angle_f融合后的角度
   float *angle_dot_f融合后的角速度
输出参数：滤波后的角度及角速度
返回值：无
**************************************************/
void kalman_filter(float angle_m, float gyro_m, float *angle_f, float *angle_dot_f)
{
angle += (gyro_m - q_bias) * dt;

Pdot=Q_angle - P - P;
Pdot = -P;
Pdot = -P;
Pdot = Q_gyro;

P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;

angle_err = angle_m - angle;

PCt_0=C_0 * P;
PCt_1=C_0 * P;

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

t_0 = PCt_0;
t_1 = C_0 * P;

P -= K_0 * t_0;
P -= K_0 * t_1;
P -= K_1 * t_0;
P -= K_1 * t_1;

angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m - q_bias;

*angle_f = angle;
*angle_dot_f = angle_dot;
}

/*************************************************

名称：void speed_filter(void)
功能：速度滤波
输入参数：无
输出参数：无
返回值：无
**************************************************/
int speed_filter(int speed)
{
u8 i;
s32 speed_sum = 0;

for(i = 1 ; i < FILTER_COUNT; i++)
{
speed_buf = speed_buf;
}

speed_buf = speed;

for(i = 0 ; i < FILTER_COUNT; i++)
{
speed_sum += speed_buf;
}

return (s16)(speed_sum / FILTER_COUNT);
}

双向PID是很好的，我没有试过，但是曾经想试！你做个，到时发个视频上来！

阿豪博士 发表于 2015-7-20 10:55:05

10xjzheng 发表于 2015-7-20 09:15
下面的计算工程的计算频率时100Hz

计算相关的函数

好的那我做一个风机吹乒乓球的。
我用激光探测乒乓球的高度然后控制风机的旋转。
我正在做电路部分。

阿豪博士 发表于 2015-7-20 11:46:25

请教一下楼主，现在困惑的一个问题是：根据MPU6050这一类的MEMS，可以得到加速度与角速度。
然后根据加速度与角速度得到一个角度姿态数值。
我们为了得到预期的姿态控制角度，例如68°，50°等等，我们是根据角度变化来进行计算比较好呢还是直接根据角速度值来进行计算比较好呢？
或许我表达的有点啰嗦，是不是这一类的计算，最终都是根据姿态的变化以及姿态的变化速率来进行PID计算的？

dory_m 发表于 2015-7-20 12:00:57

学习，谢谢！！！

10xjzheng 发表于 2015-7-20 13:36:15

阿豪博士发表于 2015-7-20 11:46
请教一下楼主，现在困惑的一个问题是：根据MPU6050这一类的MEMS，可以得到加速度与角速度。
然后根据加速度 ...

PID是差值进行计算

阿豪博士 发表于 2015-7-20 14:02:33

10xjzheng 发表于 2015-7-20 13:36
PID是差值进行计算

最理想的状态应该是PV与SV的差值为0吧。
在PID计算中，将误差设置为0比较好还是设置一个比较合适的范围比较好呢？
就是在这一部分：
//如果PID1改变了，将err都置0
if(PID->SetPointChange==1)
   {
            PID->SetPointChange=0;
         err0=err1=err2=0;
   }
//
请楼主麻烦指点一下。

gooodboooy 发表于 2015-7-20 16:51:29

MARK,收藏了先

阿豪博士 发表于 2015-7-21 09:23:31

请教一下楼主，
//返回PID的控制量，并设置占空比
SET_DUTY(speed_filter(pid2(&PID1,f_Angle,f_AnglePot)));
这里的：&PID1，PID1是另外定义了一个变量指针吗？

阿豪博士 发表于 2015-7-22 08:42:00

请教一下楼主，在上位机发送数据的时候，这个SetPointChange 如何调呢？

黑巧克力 发表于 2015-7-22 16:09:22

真是个好帖子啊，先收藏，过两天调的时候在看

阿豪博士 发表于 2015-7-23 17:53:00

PID计算中，角速度没有用到吧。为什么还要写入到PID的参数中呢……

jorry 发表于 2015-8-4 07:34:06

学习一下，顶

zengmiao 发表于 2015-8-4 18:01:23

还是比较又难度的，学习中！

KingsleyChu 发表于 2015-8-5 09:18:00

楼主可否知道下上位机发数据部分代码呢
我试了很多次没成功诶

kinsno 发表于 2015-9-10 16:08:14

10xjzheng 发表于 2015-7-16 19:22
11年国赛题目

你这个PID公式有何处出吗?

我按照标准公式推了一遍,应该是如下: Kp * e - KI * ek-1 + Kd *ek-2;
PS: 我看着有一本书,请教书名和著作人啊;

毛毛发表于 2015-9-10 17:48:49

不错。赞一个

spring152 发表于 2015-9-11 08:48:09

楼主学以致用啊，赞一个

阿豪博士 发表于 2015-9-15 15:39:45

请教一下楼主，
SetPointChange=1；
这个变量该如何控制呢？
是需要计算一次或者接收到目标值修改一次，就用到这一条指令吗？

min2008 发表于 2015-9-16 00:19:26

{:victory:}

若非发表于 2015-9-19 11:24:04

并没什么卵用，调试出来就显摆，

10xjzheng 发表于 2015-9-19 12:16:34

若非发表于 2015-9-19 11:24
并没什么卵用，调试出来就显摆，

是啊，you can you up

10xjzheng 发表于 2015-9-19 12:19:05

若非发表于 2015-9-19 11:24
并没什么卵用，调试出来就显摆，

是啊，我就是来显摆的，显摆给懂的人看啊

zcf287 发表于 2015-11-9 21:55:06

很实用谢谢分享

lihaizhao821122 发表于 2016-1-4 21:43:24

不错，真的很好！！

fengyunyu 发表于 2016-1-5 09:16:33

这个是PID调什么？

yinian 发表于 2016-1-5 09:17:42

不错的经验，谢谢分享

yao2013lin 发表于 2016-1-5 13:23:33

支持楼主收藏中

zcf287 发表于 2016-1-13 09:58:55

代码能否开源啊 {:smile:}

zcf287 发表于 2016-1-13 10:05:15

楼主，请问增量式PID算法如何处理积分分离啊？谢谢
我的处理方法：如果error_abs 小于10度,则积分因子（=积分系数*error）加到控制增量Δu(k)中，否则加0！是否正确？

TLLED 发表于 2016-1-13 10:10:48

做的很好！！！！！

10xjzheng 发表于 2016-1-13 10:13:51

zcf287 发表于 2016-1-13 10:05
楼主，请问增量式PID算法如何处理积分分离啊？谢谢
我的处理方法：如果error_abs 小于10度,则积分因 ...

还有一种：如果积分部分太大，删之。
源码部分我已经告诉你们去帮助那里获取啦，试试就知道了。

danfeidie 发表于 2016-1-13 10:15:09

学习了。这个试验很好，配合上位机。

--璞-- 发表于 2016-1-13 22:38:14

想问一下如果用cc3200做飞控可以用这个调吗？

308594151 发表于 2016-1-14 00:47:15

mark一下

zcf287 发表于 2016-1-14 09:52:10

10xjzheng 发表于 2016-1-13 10:13
还有一种：如果积分部分太大，删之。
源码部分我已经告诉你们去帮助那里获取啦，试试就知道了。 ...

谢谢了，如果（控制增量Δu(k)+上一次输出结果）超过结果限幅，下一次积分因子赋值为0。是这样吗？

dlts200466 发表于 2016-1-14 14:53:22

这个不错，MARK

xihua13104 发表于 2016-1-18 10:49:38

正在研究，MARK

yanjian305 发表于 2016-1-19 11:10:01

学习，收藏了！

chengsong 发表于 2016-5-9 10:39:26

以前电赛的时候我弄得也是这个帆板控制

xuerui3652 发表于 2016-5-9 22:45:21

请教一下楼主：
//如果PID1改变了，将err都置0
   if(PID->SetPointChange==1)
   {
            PID->SetPointChange=0;
            err=err1=err2=0;
   }
“PID->SetPointChange==1”，这个参数是基于那里在改变的呢？

fcmer2016 发表于 2016-5-11 09:34:41

顶下这个帖子，教会很多人调试方法。

小桀发表于 2016-7-5 15:19:05

本帖最后由小桀于 2016-7-5 15:20 编辑

楼主, 请问这本书叫什么名字啊, 在网上没有搜到。

10xjzheng 发表于 2016-7-29 09:52:40

小桀发表于 2016-7-5 15:19
楼主, 请问这本书叫什么名字啊, 在网上没有搜到。

https://book.douban.com/subject/6052207/
电动机的单片机控制

LiuYH 发表于 2016-8-5 12:00:57

非常好用。谢谢楼主

hellokv1688 发表于 2019-5-15 22:15:40

好资料，谢谢分享！

qq915412051 发表于 2019-5-16 09:21:30

好资料，收藏了

heyangfengyue 发表于 2019-5-21 17:55:08

谢谢楼主无私奉献啊！！！！

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

匿名上位机调节PID成功，有视频，有源码