PID.zip_PID c code_control_pid

#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; }