├── README.md ├── control_algorithm.c └── control_algorithm.h /README.md: -------------------------------------------------------------------------------- 1 | # control_algorithm 2 | PID控制算法V1.0 3 | -------------------------------------------------------------------------------- /control_algorithm.c: -------------------------------------------------------------------------------- 1 | /* 2 | * wmy_control_algorithm.c 3 | * 4 | * Created on: 2018年5月24日 5 | * Author: wmy 6 | */ 7 | 8 | #include 9 | #include 10 | #include "inc/hw_memmap.h" 11 | #include "driverlib/pin_map.h" 12 | #include "inc/hw_types.h" 13 | #include "driverlib/sysctl.h" 14 | #include "driverlib/gpio.h" 15 | #include "driverlib/pwm.h" 16 | #include "driverlib/interrupt.h" 17 | #include "driverlib/fpu.h" 18 | #include "driverlib/qei.h" 19 | #include "driverlib/uart.h" 20 | #include "utils/uartstdio.h" 21 | #include "time.h" 22 | #include "inc/hw_i2c.h" 23 | #include "driverlib/rom.h" 24 | #include "driverlib/adc.h" 25 | #include "driverlib/uart.h" 26 | #include "inc/hw_gpio.h" 27 | #include "inc/hw_ints.h" 28 | #include "string.h" 29 | #include "driverlib/timer.h" 30 | #include "wmy_control_algorithm.h" 31 | 32 | /************************************************** 33 | PID清空内部寄存器 34 | *************************************************/ 35 | void PID_Init(SPID *pid) 36 | { 37 | pid->P_TERM=0; 38 | pid->I_TERM=0; 39 | pid->D_TERM=0; 40 | pid->Integration_Sum=0; 41 | pid->LAST_Error=0; 42 | pid->NOW_Error=0; 43 | pid->Old_Position=0; 44 | pid->NOW_Out=0; 45 | pid->LAST_Out=0; 46 | pid->Out_Increment=0; 47 | } 48 | 49 | /************************************************** 50 | PID参数配置 51 | *************************************************/ 52 | void PID_Configure(SPID *pid, double kp, double ki, double kd, double setpoint) 53 | { 54 | pid->KP=kp; 55 | pid->KI=ki; 56 | pid->KD=kd; 57 | pid->SET_Point=setpoint; 58 | PID_Init(pid); 59 | } 60 | 61 | /************************************************** 62 | 位置式PID控制，开定时器使用 63 | *************************************************/ 64 | double PID_Position_Type_Control(SPID *pid, double position)//位置试PID 65 | { 66 | pid->NOW_Error = pid->SET_Point - position;//求当前误差 67 | pid->Integration_Sum = pid->Integration_Sum + pid->NOW_Error;//积分项累加 68 | pid->LAST_Error = pid->SET_Point - pid->Old_Position;//求上次误差 69 | pid->Old_Position = position; 70 | pid->P_TERM = pid->KP * pid->NOW_Error; 71 | pid->I_TERM = pid->KI * pid->Integration_Sum; 72 | pid->D_TERM = pid->KD * ( pid->NOW_Error - pid->LAST_Error ); 73 | return pid->P_TERM + pid->I_TERM + pid->D_TERM; 74 | } 75 | 76 | /************************************************** 77 | 增量式PID控制，返回值为控制增量 78 | *************************************************/ 79 | double PID_Increment_Type_Control(SPID *pid, double position)//增量试PID 80 | { 81 | pid->LAST_Out = pid->NOW_Out;//U[K-1]=U[K] 82 | pid->NOW_Error = pid->SET_Point - position;//求当前误差 83 | pid->Integration_Sum = pid->Integration_Sum + pid->NOW_Error;//积分项累加 84 | pid->LAST_Error = pid->SET_Point - pid->Old_Position;//求上次误差 85 | pid->Old_Position = position; 86 | pid->P_TERM = pid->KP * pid->NOW_Error; 87 | pid->I_TERM = pid->KI * pid->Integration_Sum; 88 | pid->D_TERM = pid->KD * ( pid->NOW_Error - pid->LAST_Error ); 89 | pid->NOW_Out = pid->P_TERM + pid->I_TERM + pid->D_TERM; 90 | pid->Out_Increment = pid->NOW_Out - pid->LAST_Out; 91 | return pid->Out_Increment; 92 | } 93 | 94 | /************************************************** 95 | 浮点数取绝对值函数 96 | *************************************************/ 97 | double Absolute_FPU(double x)//取绝对值 98 | { 99 | if(x>0) 100 | { 101 | return x; 102 | } 103 | else if(x<0) 104 | { 105 | return (-x); 106 | } 107 | return 0; 108 | } 109 | 110 | /************************************************** 111 | 一阶低通滤波算法 112 | *************************************************/ 113 | void First_Order_Low_Pass_Filter(double *value, double newvalue, double a)//a<1 a为新采样数据的比重值 114 | { 115 | *value = *value * (1-a); 116 | *value = *value + newvalue * a; 117 | } 118 | 119 | /** 120 | * Init_KalmanInfo 初始化滤波器的初始值 121 | * info 滤波器指针 122 | * Q 预测噪声方差 由系统外部测定给定 123 | * R 测量噪声方差 由系统外部测定给定 124 | */ 125 | 126 | /************************************************** 127 | 一维卡拉曼初始化 128 | *************************************************/ 129 | void Init_KalmanInfo(KalmanInfo* info, double Q, double R) 130 | { 131 | info->A = 1; //标量卡尔曼 132 | info->H = 1; // 133 | info->P = 10; //后验状态估计值误差的方差的初始值（不要为0问题不大） 134 | info->Q = Q; //预测（过程）噪声方差 影响收敛速率，可以根据实际需求给出 135 | info->R = R; //测量（观测）噪声方差 可以通过实验手段获得 136 | info->filterValue = 0;// 测量的初始值 137 | } 138 | 139 | /************************************************** 140 | 一维卡拉曼滤波器 141 | *************************************************/ 142 | double KalmanFilter(KalmanInfo* kalmanInfo, double lastMeasurement) 143 | { 144 | //预测下一时刻的值 145 | kalmanInfo->predictValue = kalmanInfo->A* kalmanInfo->filterValue; 146 | //x的先验估计由上一个时间点的后验估计值和输入信息给出，此处需要根据基站高度做一个修改 147 | //求协方差 148 | kalmanInfo->P = kalmanInfo->A*kalmanInfo->A*kalmanInfo->P + kalmanInfo->Q; 149 | //计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q 150 | kalmanInfo->preValue = kalmanInfo->filterValue; 151 | //记录上次实际坐标的值 152 | //计算kalman增益 153 | kalmanInfo->kalmanGain = kalmanInfo->P*kalmanInfo->H / (kalmanInfo->P*kalmanInfo->H*kalmanInfo->H + kalmanInfo->R); 154 | //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) 155 | //修正结果，即计算滤波值 156 | kalmanInfo->filterValue = kalmanInfo->predictValue + (lastMeasurement - kalmanInfo->predictValue)*kalmanInfo->kalmanGain; 157 | //对于一维的情况，最优估计由下式给出：x[n|n]=x[n|n-1]+K[n]*{z[n]-x[n|n-1]}。其中z[n]为观测值 158 | //利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出 X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) 159 | //更新后验估计 160 | kalmanInfo->P = (1 - kalmanInfo->kalmanGain*kalmanInfo->H)*kalmanInfo->P; 161 | //计算后验均方差 P[n|n]=(1-K[n]*H)*P[n|n-1] 162 | return kalmanInfo->filterValue; 163 | } 164 | -------------------------------------------------------------------------------- /control_algorithm.h: -------------------------------------------------------------------------------- 1 | /* 2 | * wmy_control_algorithm.h 3 | * 4 | * Created on: 2018年5月24日 5 | * Author: wmy 6 | */ 7 | 8 | #ifndef CONTROL_WMY_CONTROL_ALGORITHM_H_ 9 | #define CONTROL_WMY_CONTROL_ALGORITHM_H_ 10 | 11 | typedef struct 12 | { 13 | double KP;//比例系数 14 | double KI;//积分系数 15 | double KD;//微分系数 16 | double SET_Point;//设定值 17 | double Old_Position;//上一次的位置 18 | double Integration_Sum;//误差积分项 19 | double P_TERM;//比例控制的返回值 20 | double I_TERM;//积分控制的返回值 21 | double D_TERM;//微分控制的返回值 22 | double NOW_Error;//当前误差 23 | double LAST_Error;//上一次误差 24 | double NOW_Out; 25 | double LAST_Out; 26 | double Out_Increment; 27 | } SPID; 28 | 29 | // 一维滤波器信息结构体 30 | typedef struct{ 31 | double filterValue; //k-1时刻的滤波值，即是k-1时刻的值 32 | double kalmanGain; // Kalman增益 33 | double predictValue; //预测下一时刻的值 34 | double preValue; //记录上次实际坐标的值 35 | double A; // x(n)=A*x(n-1)+u(n),u(n)~N(0,Q) 36 | double H; // z(n)=H*x(n)+w(n),w(n)~N(0,R) 37 | double Q; //预测过程噪声偏差的方差 38 | double R; //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得) 39 | double P; //估计误差协方差 40 | } KalmanInfo; 41 | 42 | extern void PID_Init(SPID *pid); 43 | extern void PID_Configure(SPID *pid, double kp, double ki, double kd, double setpoint); 44 | extern double PID_Position_Type_Control(SPID *pid, double position);//位置试PID 45 | extern double PID_Increment_Type_Control(SPID *pid, double position);//增量试PID 46 | extern double Absolute_FPU(double x);//取绝对值 47 | extern void First_Order_Low_Pass_Filter(double *value, double newvalue, double a);//a<1 a为新采样数据的比重值 48 | extern void Init_KalmanInfo(KalmanInfo* info, double Q, double R); 49 | extern double KalmanFilter(KalmanInfo* kalmanInfo, double lastMeasurement); 50 | 51 | #endif /* CONTROL_WMY_CONTROL_ALGORITHM_H_ */ 52 | --------------------------------------------------------------------------------