增量式PID算法C++源码 - - Powered by Discuz!

#include <iostream>
#include <stdio.h>
#include <math.h>
#include <Windows.h>
//定义PID变量的结构体
struct _pid
{
float SetSpeed;
float ActualSpeed;
float err;
float err_last; //定义上上个偏差
float err_next; //定义上一个偏差值
float Kp,Ki,Kd;
}pid;
//初始化变量
void PID_init(void)
{
printf ("pid_value_init_begin\n");
pid.ActualSpeed = 0.0;
pid.SetSpeed = 0.0;
pid.err = 0.0;
pid.err_last = 0.0;
pid. err_next = 0.0;
pid.Kp = 0.2;
pid.Ki = 0.015;
pid.Kd = 0.2;
printf("pid_init end \n");
}
//增量式算法
float PID_calculate(float speed)
{
pid.SetSpeed = speed;
pid.err = pid.SetSpeed - pid.ActualSpeed;
float incrementSpeed = pid.Kp * (pid.err - pid.err_next) + pid.Ki * pid.err + pid.Kd * (pid.err - 2 * pid.err_next + pid.err_last);
pid.ActualSpeed += incrementSpeed;
pid.err_last = pid.err_next; //定义上一个偏差值
pid.err_next = pid.err;
return pid.ActualSpeed;
}
int main(void)
{
int setcount = 0;
printf ("Please begin \n");
PID_init();
while (setcount < 100)
{
float speed = PID_calculate(250.0);
printf ("value is %f\n",speed);
setcount++;
Sleep(300);
}
system("pause");
return 0;
}

复制代码