位置型PID算法的C语言实现

位置型PID算法

/*
	位置型pid
*/
#include <stdio.h>
//#include<ioctl.h>

struct _pid{
float SetSpeed; 		//定义设定值
float ActualSpeed; 		//定义实际值
float err; 				//定义偏差值
float err_last; 		//定义上一个偏差值
float Kp,Ki,Kd; 		//定义比例、积分、微分系数
float voltage; 			//定义电压值（控制执行器的变量）
float integral;		    //定义积分值
}pid;

//项目中获取到的参数
void PID_init(){
	printf("PID_init begin \n");
	pid.SetSpeed=0.0;
	pid.ActualSpeed=0.0;
	pid.err=0.0;
	pid.err_last=0.0;
	pid.voltage=0.0;
	pid.integral=0.0;
	pid.Kp=0.01;				//自己设定
	pid.Ki=0.115;			//自己设定
	pid.Kd=0.35;				//自己设定
	printf("PID_init end \n");
}

float PID_realize(float speed){
	pid.SetSpeed=speed;						//设定值
	pid.err=pid.SetSpeed-pid.ActualSpeed;	//设定值-实际值
	pid.integral+=pid.err;					//积分值，偏差累加
	pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
	pid.err_last=pid.err;					//上一个偏差值
	pid.ActualSpeed=pid.voltage*1.0;		//算出实际值
	return pid.ActualSpeed;					//返回
}

void delay(int i)
{
     while(i--);
}
int main(){
	printf("System begin \n");
	PID_init();
	int count=0;
	while(count<100)
	{
        float  speed=PID_realize(3289);
        printf("%f\n",speed);
        count++;
	}
	return 0;
}