#include <stdio.h>
struct _pid
{
float error; //差值
float error_last; //上一次差值
float integral; //差值的和
}
float Kp =0.01; //比例系数, 提升速度
float Ki = 1; //积分系数，稳定输出
float Kd =0.01; //微分系数，减小振荡
float cur_speed = 0; //当前的速度
float level = 0; //用于输出pwm
float set_speed = 50; //设定的速度
struct _pid pid;
void pid_init()
{
pid.error = 0;
pid.error_last = 0;
pid.integral = 0;
}
void pid_control()
{
pid.error=set_speed-cur_speed;
pid.integral +=pid.error;
level =Kp*pid.error+Ki*pid.integral+Kd*(pid.error-pid.error_last);
pid.error=pid.error_lasst;
}
int main()
{
pid_init();
for(i=0;i<10;i++)
{
pid_control();
printf("当前速度%f 等级 %f\n",cur_speed,level);
}
return 0 ;
}
|
楼主
标题置顶
标题高亮
