漫谈PID——实现与调参

 1 //==================
 2 //PID.H
 3 //==================
 4 
 5 
 6 #ifndef __PID_H
 7 #define __PID_H
 8 
 9 
10 //PID计算需要的参数
11 typedef struct pid
12 {
13      float ref;     //系统待调节量的设定值 
14      float fdb;  //系统待调节量的反馈值，就是传感器实际测量的值
15      
16       
17      float KP;                 //比例系数
18      float KI;                 //积分系数
19      float KD;                 //微分系数
20      
21      float T;        //离散化系统的采样周期 
22      
23      float a0;         //变量代替三项的运算结果
24      float a1;
25      float a2;
26      
27      float error;    //当前偏差e（k）
28      float error_1; //前一步的偏差
29      float error_2; //前前一步的偏差
30      
31      float output;      //pid控制器的输出
32      float output_1;      //pid的前一步输出
33      float out_max;      //输出上限
34      float out_min;      //输出下限
35      
36      
37 }PID_value;                     //定义一个PID_value类型，    此时PID_value为一个数据类型标识符，数据类型为结构体
38 
49 
50 //条件编译的判别条件，用于调试
51 #define PID_DEBUG  1
52 
53 //pid函数声明
54 void PID_operation(PID_value *p);
55 void PID_out(void);
56 
57 float constrain_float(float amt, float low, float high);      //浮点数限幅
58 //int constrain_int16(int amt, int low, int high);            //整型数限幅
59 
60 #endif
61 
62 //========================
63 //END OF FILE
64 //========================

  1 //=============
  2 //PID.C
  3 //=============
  4 
  5 #include "main.h"
  6 
  7 
  8 #define set_distance 10.00  //设定距离
  9 #define allow_error 1.0    //死区，允许误差带,0.5太小，系统不稳定，一直在调节，2就太大，
 10 
 11 extern float real_distance ;      //实际距离
 12 extern PID_value  xdata ASR  ;      
 13 
 14 
 15 /* ********************************************************
 16 ** 作者    ：Andrew
 17 ** 日期       ：2018.3.8    
 18 ** 说明：
 19     1、PID默认为PI调节器
 20     2、使用了条件编译进行功能切换，节省计算时间
 21        在校正PID参数的时候，将宏定义 PID_DEBUG 设为1；
 22        校正完毕后，置0；
 23     3、同时在初始化的时候直接为a0,a1,a2赋值                              
 24 ******************************************************** */
 25 void PID_operation(PID_value *p)
 26 {
 27     
 28     //使用条件编译进行功能切换
 29     #if (PID_DEBUG)
 30         
 31     float a0,a1,a2;
 32     
 33     //计算中间变量a0,a1,a2;
 34     a0 = p->KP + p->KI*p->T + p->KD/p->T ;
 35     a1 = p->KP + 2*p->KD/p->T ;
 36     a2 = p->KD/p->T ;
 37     //计算输出
 38     p->output = p->output_1 + a0*p->error - a1*p->error_1 + a2*p->error_2 ;
 39     
 40     #else
 41     //非调试状态下，直接给a赋值计算输出，减小计算量,因为一旦三个系数确定，T已知，即可自己计算出对应的a
 42     p->output = p->output_1 + p->a0*p->error - p->a1*p->error_1 + p->a2*p->error_2 ;
 43     
 44     #endif
 45     
 46     //输出限幅
 47     p->output = constrain_float(p->output,p->out_min,p->out_max);
 48     
 49     //为下次计算迭代
 50     //这里顺序千万不要搞错，不然输出占空比是错误的。
 51     p->output_1 = p->output;
 52     p->error_2 = p->error_1;
 53     p->error_1 = p->error;
 54     
 55 }
 56 
 57 /* ********************************************************
 58 ** 作者    ：Andrew
 59 ** 日期       ：2018.3.8    
 60 ** 说明：
 61     1、首先根据设定与实际的距离差，判断需要前进还是后退
 62     2、在误差为     allow_error 之内，停车，防止小车一直在抖，毕竟超声波有误差。                        
 63 ******************************************************** */
 64 void PID_out(void)
 65 {
 66     float xdata duty;
 67     
 68     ASR.ref = set_distance;        //距离给定
 69     ASR.fdb = real_distance;    //获取实际距离反馈
 70     
 71     ASR.error = ASR.ref - ASR.fdb;    //偏差
 72     
 73     PID_operation(&ASR);
 74     
 75     duty = ASR.output;
 76     
 77     if(ASR.error > allow_error)        //设定值大于实际值，小车需要后退
 78     {
 79         left_go_back;
 80         right_go_back;
 81         Left_Forward_Duty = (int)Low_Speed + (int)duty;        //带符号数和无符号数运算会转换为无符号数
 82         Right_Forward_Duty = Left_Forward_Duty;        //速度一样
 83     }
 84     else if((-ASR.error) > allow_error)    //设定值小于实际值，小车前进
 85     {
 86         left_go_ahead;
 87         right_go_ahead;
 88         Left_Forward_Duty = (int)Low_Speed + (int)duty;
 89         Right_Forward_Duty = Left_Forward_Duty ;        //速度一样
 90     }
 91     else            //在误差范围内，停车
 92         car_stop();
 93 }
 94 
 95 
 96 //浮点数限幅,constrain ->约束，限制
 97 //如果输入不是数字，则返回极端值的平均值
 98 //isnan函数检测输入是否是数字，is not a number
 99 float constrain_float(float amt, float low, float high) 
100 {
101 //    if (isnan(amt))                  //51里面没有这个库函数，需要自己实现
102 //    {
103 //        return (low+high)*0.5f;
104 //    }
105     return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
106 }
107 
108 /*
109 //16位整型数限幅
110 int constrain_int16(int amt, int low, int high)
111 {
112     return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
113 }
114 */
115 
116 //========================
117 //END OF FILE
118 //========================

    //初始化PI调节器参数全为0, 该函数在string.h
    memset(&ASR, 0, sizeof(PID_value));

    ASR.KP = distance_kp;
    ASR.KI = distance_ki;
    ASR.KD = distance_kd;
    ASR.T = T0;
    ASR.out_max = limit_max;
    ASR.out_min = limit_min;