pid.cpp

来自「PID control source code」· C++ 代码 · 共 103 行

#include <iostream>
#include <cmath>


using namespace std;

typedef double real;

class PID_Controller
{
public:
	void Initialize(real kp, real ki, real kd, real error_thresh, real step_time);
	real Update(real error);

private:
	bool m_started;
	real m_kp, m_ki, m_kd, m_h, m_inv_h, m_prev_error, m_error_thresh, m_integral;
};

void PID_Controller::Initialize(real kp, real ki, real kd, real error_thresh, real step_time)
{
	m_kp=kp;
	m_ki=ki;
	m_kd=kd;
	m_prev_error = 0;
	m_error_thresh=error_thresh;

	m_h=step_time;
	m_inv_h=1/step_time;

	m_integral=0;
	m_started=false;
}

real PID_Controller::Update(real error)
{
	real q;
	if(fabs(error)<m_error_thresh)				//integrator windup
		q=1;
	else
		q=0;

	m_integral+=m_h*q*error;

	real deriv;
	if(!m_started)
	{
		m_started=true;
		deriv=0;
	}
	else deriv=(error-m_prev_error)*m_inv_h;
	
	m_prev_error=error;

	return m_kp*(error+m_ki*m_integral+m_kd*deriv);
}

int main()
{
	//int prev_value, next_value;
	int repeat=1;
	double feedback_value;
	double output_value=0;	
	real parameter[6];

	char* variable[6] = {"kp","ki","kd","error thresh","step time","Reference"};
	
	PID_Controller My_PID;
	

	while(1)
	{
		for(int i=0;i<6;i++)
		{
			cout << "Insert value of \"" << variable[i] << "\"" <<endl;
			cin >> parameter[i];
			cout << i+1 << "th parameter = " << parameter[i] <<endl <<endl;
		}
		
		My_PID.Initialize(parameter[0],parameter[1],parameter[2],parameter[3],parameter[4]);
		feedback_value = parameter[5]; 
		cout << endl << "reference = "<< parameter[5] <<endl<<endl<<endl;
	
		for (int j=0;j<20;j++)
		{
			cout << "e(t) = " << parameter[5] <<" - " 
				<< output_value << " = " << feedback_value <<"           ";

			output_value = My_PID.Update(feedback_value);
			cout << j+1 <<"th Output = " << output_value << endl;
			feedback_value = parameter[5] - output_value;

		}


	}

	return 0;

}