PID控制算法的C語言實現四 增量型PID的C語言實現

/*-------------------------------------------
    2 位置型PID C實現(控制電機轉速)
--------------------------------------------*/

//(1)定義PID 結構體變量
struct pid
{
	float SetSpeed;    //設定速度
	float ActualSpeed;      //實際值
	float err;      //k,定義偏差值
	float err_last;       //k-1,上一個偏差值
	float err_last_next;  //k-2
	float Kp, Ki, Kd;     //p,i,d系數
	
}pid;



int main()
{
	int count = 0;

	cout << "Please begin \n";
	pid_value_init();

	while (count < 100)
	{
		float speed = PID_realize(200.0);

		cout << "value is " << speed << endl;
		count++;
	}

	system("pause");
}



//(3) 控制算法注意：這里用了最基本的算法實現形式，沒有考慮死區問題，
//沒有設定上下限，只是對公式的一種直接的實現，后面的介紹當中還會逐漸的對此改進。
float PID_realize(float speed)
{
	float incrementSpeed;
	pid.SetSpeed = speed;
	pid.err = pid.SetSpeed - pid.ActualSpeed;
	

	incrementSpeed = pid.Kp * (pid.err -pid.err_last ) + pid.Ki*pid.err + pid.Kd*(pid.err -2* pid.err_last + pid.err_last_next);

	pid.ActualSpeed += incrementSpeed;
	pid.err_last = pid.err;
	pid.err_last_next = pid.err_last;
	return pid.ActualSpeed;
}

//(2) 初始化變量
void pid_value_init(void)
{
	cout << "pid_value_init begin \n" << endl;
	system("pause");

	pid.SetSpeed = 0;
	pid.ActualSpeed = 0;
	pid.err = 0;
	pid.err_last = 0;
	pid.err_last_next = 0;

	pid.Kp = 0.1;
	pid.Ki = 0.15;
	pid.Kd = 0.1;

	cout << "pid_value_init end \n" << endl;
	system("pause");
}