PID 算法的 C 语言实现
[cpp]view plaincopy
1 float PIDcal(float setpoint,float actual_position)
2 {
3 static float pre_error = 0; // 定义前一次误差
4 static float integral = 0; // 定义积分项累加值
5 float error; // 定义最新误差
6 float derivative; // 定义微分项
7 float output; // 定义输出
8
9 // 计算误差
10 error = setpoint - actual_position;
11
12 // 死区设置,在误差极小的情况下不做积分处理,保证稳定性
13 if(my_abs(error) > epsilon)
14 {
15 integral = integral + error*dt; // 积分项,做完壁障之后,积分清零
16 }
17
18 derivative = (error - pre_error)/dt; // 微分项
19
20 output = Kp*error + Ki*integral + Kd*derivative; // 各项乘以系数得到
PID 输出
21
22 // 输出限幅,确保输出不会太夸张
23 if(output > MAX)
24 {
25 output = MAX;
26 }
27 else if(output < MIN)
28 {