//==================

 //PID.H

 //==================

 #ifndef __PID_H

 #define __PID_H

 //PID计算需要的参数

 typedef struct pid

 {

      float ref;     //系统待调节量的设定值

      float fdb;  //系统待调节量的反馈值，就是传感器实际测量的值

      float KP;                 //比例系数

      float KI;                 //积分系数

      float KD;                 //微分系数

      float T;        //离散化系统的采样周期 

      float a0;         //变量代替三项的运算结果

      float a1;

      float a2;

      float error;    //当前偏差e（k）

      float error_1; //前一步的偏差

      float error_2; //前前一步的偏差

      float output;      //pid控制器的输出

      float output_1;      //pid的前一步输出

      float out_max;      //输出上限

      float out_min;      //输出下限

 }PID_value;                     //定义一个PID_value类型，    此时PID_value为一个数据类型标识符，数据类型为结构体

 //条件编译的判别条件，用于调试

 #define PID_DEBUG  1

 //pid函数声明

 void PID_operation(PID_value *p);

 void PID_out(void);

 float constrain_float(float amt, float low, float high);      //浮点数限幅

 //int constrain_int16(int amt, int low, int high);            //整型数限幅

 #endif

 //========================

 //END OF FILE

 //========================

 //=============

 //PID.C

 //=============

 #include "main.h"

 #define set_distance 10.00  //设定距离

 #define allow_error 1.0    //死区，允许误差带,0.5太小，系统不稳定，一直在调节，2就太大，

 extern float real_distance ;      //实际距离

 extern PID_value  xdata ASR  ;      

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

 ** 作者    ：Andrew

 ** 日期       ：2018.3.8

 ** 说明：

     1、PID默认为PI调节器

     2、使用了条件编译进行功能切换，节省计算时间

        在校正PID参数的时候，将宏定义 PID_DEBUG 设为1；

        校正完毕后，置0；

     3、同时在初始化的时候直接为a0,a1,a2赋值

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

 void PID_operation(PID_value *p)

 {

     //使用条件编译进行功能切换

     #if (PID_DEBUG)

     float a0,a1,a2;

     //计算中间变量a0,a1,a2;

     a0 = p->KP + p->KI*p->T + p->KD/p->T ;

     a1 = p->KP + *p->KD/p->T ;

     a2 = p->KD/p->T ;

     //计算输出

     p->output = p->output_1 + a0*p->error - a1*p->error_1 + a2*p->error_2 ;

     #else

     //非调试状态下，直接给a赋值计算输出，减小计算量,因为一旦三个系数确定，T已知，即可自己计算出对应的a

     p->output = p->output_1 + p->a0*p->error - p->a1*p->error_1 + p->a2*p->error_2 ;

     #endif

     //输出限幅

     p->output = constrain_float(p->output,p->out_min,p->out_max);

     //为下次计算迭代

     //这里顺序千万不要搞错，不然输出占空比是错误的。

     p->output_1 = p->output;

     p->error_2 = p->error_1;

     p->error_1 = p->error;

 }

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

 ** 作者    ：Andrew

 ** 日期       ：2018.3.8

 ** 说明：

     1、首先根据设定与实际的距离差，判断需要前进还是后退

     2、在误差为     allow_error 之内，停车，防止小车一直在抖，毕竟超声波有误差。

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

 void PID_out(void)

 {

     float xdata duty;

     ASR.ref = set_distance;        //距离给定

     ASR.fdb = real_distance;    //获取实际距离反馈

     ASR.error = ASR.ref - ASR.fdb;    //偏差

     PID_operation(&ASR);

     duty = ASR.output;

     if(ASR.error > allow_error)        //设定值大于实际值，小车需要后退

     {

         left_go_back;

         right_go_back;

         Left_Forward_Duty = (int)Low_Speed + (int)duty;        //带符号数和无符号数运算会转换为无符号数

         Right_Forward_Duty = Left_Forward_Duty;        //速度一样

     }

     else if((-ASR.error) > allow_error)    //设定值小于实际值，小车前进

     {

         left_go_ahead;

         right_go_ahead;

         Left_Forward_Duty = (int)Low_Speed + (int)duty;

         Right_Forward_Duty = Left_Forward_Duty ;        //速度一样

     }

     else            //在误差范围内，停车

         car_stop();

 }

 //浮点数限幅,constrain ->约束，限制

 //如果输入不是数字，则返回极端值的平均值

 //isnan函数检测输入是否是数字，is not a number

 float constrain_float(float amt, float low, float high)

 {

 //    if (isnan(amt))                  //51里面没有这个库函数，需要自己实现

 //    {

 //        return (low+high)*0.5f;

 //    }

     return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));

 }

 /*

 //16位整型数限幅

 int constrain_int16(int amt, int low, int high)

 {

     return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));

 }

 */

 //========================

 //END OF FILE

 //========================

    //初始化PI调节器参数全为0, 该函数在string.h

    memset(&ASR, , 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;