C语言编写PID基础算法

最新推荐文章于 2024-07-25 12:55:52 发布

Hello_Mr.DJ

最新推荐文章于 2024-07-25 12:55:52 发布

阅读量1.7k

点赞数 1

文章标签： c语言算法开发语言

本文链接：https://blog.csdn.net/m0_57068768/article/details/126746789

版权

PID的作用为将系统输出快速稳定于某一个值

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/*pid的作用是将某一个需要稳定的变量在最快的时间内做到稳定
对于平衡小车就是将陀螺仪传感器的传感值稳定在平衡位置，系统的性能使用
误差和误差变化率来标识，系统的稳定性需要调整比例，积分和微分的系数值*/

#define static_speed 200
#define speed_gap 180

struct pid_parameter{
	float SetSpeed;     //定义设定速度
	float ActualSpeed;  //定义实际值
	float err;			//定义与期望标准值的偏差
	float last_err;     //定义上一个偏差值
	//Kp作用加快系统响应速度，Ki作用是消减系统的稳态误差，Kd作用是改善动态特性
	float Kp,Ki,Kd;     //定义比例，积分，微分系数
	float voltage;		//定义输出电压值
	float integral;     //定义积分值
	float umax;
	float umin;
}pid;


void Pid_Init(void)
{
	printf("PID Init begin!\n");
	pid.SetSpeed = 0.0;
	pid.ActualSpeed = 0.0;
	pid.err = 0.0;
	pid.last_err = 0.0;
	pid.voltage = 0.0;
	pid.integral = 0.0;
	pid.Kp = 0.4;      //算法比例，积分，微分系数需要后调
	pid.Ki = 0.2;
	pid.Kd = 0.2;
	pid.umax = 400;
	pid.umin = -200;
	printf("PID Init end!\n");
}


float Pid_Realize(float speed){
	int index;
	pid.SetSpeed = speed;
	pid.err = pid.SetSpeed-pid.ActualSpeed;
    //防止启动或者结束的时候误差过大以及变积分控制，
	//err越大积分越慢，err越小积分越快
	if(fabsf(pid.err) > speed)
		index = 0;
	else{
		if(fabsf(pid.err) > speed_gap)
			index = (static_speed - fabsf(pid.err))/20;
		else{
			index = 1;
		}
		
		//进入负饱和区，只积正值,进入正饱和区,只积负值
		if(pid.ActualSpeed > pid.umax)
		{
			if(pid.err < 0)
				pid.integral += pid.err;
		}
		else if(pid.ActualSpeed < pid.umin)
		{
			if(pid.err > 0)
				pid.integral += pid.err;
		}
		else{
			pid.integral += pid.err;
		}
	}
	
	//pid算法
	pid.voltage = pid.Kp*pid.err + index*pid.Ki*pid.integral + pid.Kd*(pid.err - pid.last_err);
	pid.last_err = pid.err;
	//速度与电压等值
	pid.ActualSpeed = pid.voltage * 1.0;
	return pid.ActualSpeed;
}


int main(void)
{
	int count = 0;
	float speed;
	printf("System begin!\n");
	Pid_Init();
	while(count<500)
	{
		speed = Pid_Realize(static_speed);
		printf("%f\n",speed);
		count++;
	}
	return 0;
}