STM32运用之自平衡小车的PID算法封装-求加精

低调的华丽

继卡尔曼封装之后推出第二个封装，PID算法，这个其实比上个简单多了下面贴出代码：

1. /**
   
2.   ******************************************************************************
   
3.   * @file    PID_Control.h
   
4.   * @author  willieon
   
5.   * @version V0.1
   
6.   * @date    January-2015
   
7.   * @brief   PID控制算法头文件
   
8.   *                        定义结构体类型以及声明函数
   
9.   *                        #define IF_THE_INTEGRAL_SEPARATION  0/1  为积分分离标志
   
10.   ******************************************************************************
    
11.   **/
    
12. 
    
13. #ifndef __PID_CONTROL_H__
    
14. #define __PID_CONTROL_H__
    
15. 
    
16. #define IF_THE_INTEGRAL_SEPARATION  0   
    
17. //#define IF_THE_INTEGRAL_SEPARATION  1   //是否积分分离  0-不分离，1 -分离
    
18. 
    
19. typedef struct
    
20. {
    
21.         double SetPoint; // 设定目标 Desired Value   
    
22.         double Proportion; // 比例常数 Proportional Const
    
23.         double Integral; // 积分常数 Integral Const
    
24.         double Derivative; // 微分常数 Derivative Const   
    
25.         double LastError; // Error[-1]
    
26.         double PrevError; // Error[-2]
    
27.         double SumError; // Sums of Errors  
    
28. }PID;
    
29. 
    
30. #if IF_THE_INTEGRAL_SEPARATION            //是否积分分离预编译开始
    
31. 
    
32. double PIDCalc(double NextPoint ,double SepLimit, PID *pp);   //带积分分离的PID运算
    
33. 
    
34. #else
    
35. 
    
36. double PIDCalc( double NextPoint, PID *pp);     //不带积分分离的PID运算
    
37. 
    
38. #endif        //是否积分分离预编译结束
    
39. 
    
40. void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp);
    
41. 
    
42. #endif
    

复制代码

1. /**
   
2.   ******************************************************************************
   
3.   * @file    PID_Control.c
   
4.   * @author  willieon
   
5.   * @version V0.1
   
6.   * @date    January-2015
   
7.   * @brief   PID控制算法函数代码
   
8.   *        
   
9.   *
   
10.   ******************************************************************************
    
11.   **/
    
12. 
    
13. #include "PID_Control.h"
    
14. #include "math.h"
    
15. 
    
16. /*************************************************************************************
    
17. *        名    称： double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
    
18. *        功    能： PID控制运算
    
19. *        入口参数： PID *pp  - 定义的运算所需变量的结构体
    
20. *                           NextPoint - 负反馈输入值
    
21. *                           SepLimit  - 积分分离上限
    
22. *        出口参数： 返回PID控制量
    
23. *        说    明： 默认不进行积分分离，如果用户需要使用积分分离，需在PID_Control.h中
    
24. *                                将 #define IF_THE_INTEGRAL_SEPARATION  0  改为
    
25. *                            #define IF_THE_INTEGRAL_SEPARATION  1
    
26. *        调用方法： 进行积分分离时入口参数为3个，具体方法如下：
    
27. *                                PID PIDControlStruct ;   //定义PID运算结构体
    
28. *                                PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//结构体初始化，注意&符号不能省
    
29. *                                ControlData = PIDCalc(ReadData, 200, &PIDControlStruct);   //控制量 = PIDCalc(反馈值,积分分离上限,PID运算结构体)
    
30. *
    
31. ***************************************************************************************
    
32. */
    
33. 
    
34. #if IF_THE_INTEGRAL_SEPARATION
    
35. 
    
36. double PIDCalc(double NextPoint ,double SepLimit, PID *pp)
    
37. {
    
38.         double dError, Error,Flag;   
    
39.         Error = pp->SetPoint - NextPoint;         // 偏差
    
40.         if(abs(Error) > SepLimit)        //当偏差大于分离上限积分分离
    
41.         {
    
42.                 Flag = 0;
    
43.         }
    
44.         else       //当偏差小于分离上限，积分项不分离
    
45.         {
    
46.                 Flag = 1;
    
47.                 pp->SumError += Error;         // 积分  
    
48.         }
    
49.         dError = pp->LastError - pp->PrevError;         // 当前微分
    
50.         pp->PrevError = pp->LastError;
    
51.         pp->LastError = Error;  
    
52.         return (
    
53.                 pp->Proportion                *                Error                 // 比例项
    
54.                 + Flag * pp->Integral        *                pp->SumError         // 积分项
    
55.                 + pp->Derivative                *                dError                 // 微分项
    
56.                 );
    
57. }
    
58. 
    
59. #else
    
60. 
    
61. double PIDCalc( double NextPoint, PID *pp)
    
62. {  
    
63.         double dError, Error;   
    
64.         Error = pp->SetPoint - NextPoint;                         // 偏差
    
65.         pp->SumError += Error;                                        // 积分  
    
66.         dError = pp->LastError - pp->PrevError;                // 当前微分
    
67.         pp->PrevError = pp->LastError;
    
68.         pp->LastError = Error;  
    
69.         return (pp->Proportion        *        Error                // 比例项
    
70.                 + pp->Integral                *        pp->SumError         // 积分项
    
71.                 + pp->Derivative        *        dError         // 微分项
    
72.                 );
    
73. }
    
74. 
    
75. #endif
    
76. 
    
77. 
    
78. /*************************************************************************************
    
79. *        名    称： double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
    
80. *        功    能： PID初始化设定
    
81. *        入口参数： PID *pp  - 定义的运算所需变量的结构体
    
82. *                           SetPoint - 设定的目标值
    
83. *                           Proportion，Integral ，Derivative - P,I,D系数
    
84. *        出口参数： 无
    
85. *        说    明：        
    
86. *        调用方法：  PID PIDControlStruct ;   //定义PID运算结构体
    
87. *                                PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//结构体初始化，注意&符号不能省
    
88. *                                因为函数需要传入一个指针，需要对结构体取首地址传给指针
    
89. *
    
90. ***************************************************************************************
    
91. */
    
92. 
    
93. 
    
94. void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp)
    
95. {  
    
96.         pp -> SetPoint = SetPoint; // 设定目标 Desired Value   
    
97.         pp -> Proportion = Proportion; // 比例常数 Proportional Const
    
98.         pp -> Integral = Integral; // 积分常数 Integral Const
    
99.         pp -> Derivative = Derivative; // 微分常数 Derivative Const   
    
100.         pp -> LastError = 0; // Error[-1]
     
101.         pp -> PrevError = 0; // Error[-2]
     
102.         pp -> SumError = 0; // Sums of Errors
     
103. 
     
104.         //memset ( pp,0,sizeof(struct PID));   //need include "string.h"
     
105. }

复制代码

低调的华丽

1. 
   

复制代码

低调的华丽

将前篇  卡尔曼  和本次的 PID放在一起，在VS2010平台中调试 代码如下  

1. #include "kalman.h"
   
2. 
   
3. #include "stdio.h"
   
4. #include "stdlib.h"
   
5. #include "PID_Control.h"
   
6. 
   
7. void main(void)
   
8. 
   
9. {
   
10.         KalmanCountData k;
    
11.         PID PIDControlStruct;
    
12.         Kalman_Filter_Init(&k);
    
13.         PIDInit(50, 1, 0.04, 0.2, &PIDControlStruct);
    
14.         int m,n;
    
15. 
    
16.         double out;
    
17. 
    
18.         for(int a = 0;a<80;a++)
    
19.         {
    
20.                 m = 1+ rand() %100;
    
21.                 n = 1+ rand() %100;
    
22.                 Kalman_Filter((float)m,(float)n,&k);
    
23.                 out = PIDCalc(k.Angle_Final, &PIDControlStruct);
    
24.                 printf("%3d and %3d is %6f -pid- %6f\r\n",m,n,k.Angle_Final,out);
    
25. 
    
26.         }
    
27. }
    

复制代码

低调的华丽

如果觉得double类型对CPU负载太大，就改为float吧，有时候控制量范围不大，float完全能满足！

低调的华丽

看本帖的反响，如果支持高，我会把手头自平衡小车的代码用标准的库格式封装改写。逐步放出来

Marcello

好东西。。。。

维克多

看过 来支持下!!!!!

yy2816157676

实现的情况如何呢，稳定不

低调的华丽

yy2816157676 发表于 2015-1-11 09:28
实现的情况如何呢，稳定不

稳定不稳定不是主要看代码，而是看整定啊

唐顺才

低调的华丽 发表于 2015-1-10 18:45
看本帖的反响，如果支持高，我会把手头自平衡小车的代码用标准的库格式封装改写。逐步放出来

在精神和灵魂上支持你

唐顺才

这种算法实际应用中很有用，大家顶起来，版主加油！！！

yuanqingquan13

学习一下

liwwn

学习啦。。。。。

cupid1988li

一直想学习PID怎么搞，正好学习一下

xyc2690

平衡车做的怎么样了？

低调的华丽

xyc2690 发表于 2015-1-23 16:36
平衡车做的怎么样了？

香蕉电机的做完了，感觉性能不行，现在在做步进电机的

1593996796

看看













。

满地落叶

好，正需要。谢谢楼主

sealtech

学习才是王道

（苦）行僧

哪里有PI算法

（苦）行僧

这些代码看不懂啊，唉

（苦）行僧

有木有简单的陀螺仪积分的代码。

bjyy

路过，赞一个

479220091

快快快看。。。

duowanjacky

mark！！！

hyc1014

学习学习，谢谢楼主

temple

顶一个，我最近也在弄自平衡，在pid算法这卡住了

惜往何处

楼主好叼，前来膜拜膜拜

威威威

最近想搞一个平衡小车，学学先

1015439332

                                           

vh520

学习 最近做平衡车做的好烦

mengxiangjia_xi

看看大神们的心血

小菜鸟

在学习。。。。。。。。

yhtractor

和倒立摆控制方法类似吗？

fierce

隐藏内容是什么？

cy133

good!!!!!!!!!!!!!!!!!!!!!!!!

疯子教授

pid是什么？？

等风来

看看，学习学习

傲视灬果果

                                                  kk

不再颓废24

希望对我学习PID有帮助         

abc158

居然 是卡尔曼一起的

lanydwing

回复看看         

lanydwing

其实参数参数整定是很虐心的一件事、、、、

yinhao

好东西，版主加油！

sxdahss

guolai 学习一下

maomao3784

牛               

初心

楼主好样的

flyfox8

LZ辛苦了。

yhking

看看。。。。。。

月寒剑心

6666666666666666666666666666666666666666