#include<stdio.h>
#include<math.h>
void main()
{
	double tao;    //系統純延時參數，一般為采樣時間的整數倍
    double ts;    //采樣時間
    int n;    //n=tao/ts
    
    double kp,ki,kd;    //PID控制器參數
    double error_1,error_2;    //，前1、2次的誤差，在增量PID算法中需要error_2
    double error,perror,ierror,derror;    //用于計算PID輸出u的參數
    double den_1,den_2,den_3;    //Z傳函分母的系數
    double num_1,num_2,num_3;    //Z傳函的分子系數
    double yout[500],rin,yout_1,yout_2,yout_3,u_1,u_2,u_3; 
	//系統輸入、系統輸出、PID控制器輸出; 
	for(int i=0;i<=500;i++)
	{
		yout[i]=0;
	
	}


	//PID參數初始化 G(s)=m_k*exp(-tao*s)/[(m_t1s+1)(m_t2s+1)]
    double m_t1,m_t2,m_k,tao0,e[500];
	double x=0.0,A=0.0,T,k0,ti,td;
	printf("input m_t1:  m_t1=");
	scanf("%lf",&m_t1);
	printf("input m_t2:  m_t2=");
	scanf("%lf",&m_t2);
	printf("input m_k:  m_k=");
	scanf("%lf",&m_k);
	printf("input tao:  tao=");
	scanf("%lf",&tao);
	printf("input ts:  ts=");
	scanf("%lf",&ts);
	

     n=(int)(tao/ts+0.5);
     double u[100];
     error_1=0.0;
     error_2=0.0;
     error=0.0;
     perror=0.0;
     ierror=0.0;
     derror=0.0;
    double z1,z2,b,c;
    if (m_t1==m_t2)
	{
    z1=exp(-(ts/m_t1));      //e(-T/T1) z2=z1    pid->num_1=0;
    num_2=m_k*(1-z1-ts*z1/m_t1);
    num_3=m_k*(z1*z1-z1+ts*z1/m_t1);    
	den_1=1;
    den_2=-2*z1;
    den_3=z1*z1; 
	} 
    else
    {
    z1=exp(-(ts/m_t1));      //e(-T/T1)
    z2=exp(-(ts/m_t2));      //e(-T/T2)
    b=m_t1/(m_t2-m_t1);
    c=m_t2/(m_t1-m_t2);    
	num_1=0;
    num_2=m_k*(1+b*z1+c*z2);
    num_3=m_k*(z1*z2+b*z2+c*z1);    
	den_1=1;
    den_2=-(z1+z2);
    den_3=z1*z2; 
    }
   
  
  for(int j=0;j<n+2;j++)
  {
    u[j]=0.0;
  }

  num_2=0.058824;
  num_3=0.0;

  //參數整定
  int a=500;
  printf("input 階躍信號 rin:  rin=");
  scanf("%lf",&rin);//階躍信號
  for(int k=0;k<=a;k++)
	{
		yout[k]=den_2*yout_1+den_3*yout_2+num_2*u[n];//+num_3*u[n+1];
        error=rin-yout[k];
		
		 for (int j=n+1;j>0;j--)
         {
             u[j]=u[j-1];
         }
         u[0]=error;
	
		 yout_3=yout_2;
		 yout_2=yout_1;
		 yout_1=yout[k];
		 e[k]=yout_1-yout_2;
         x=e[k]*k*ts;
		 A=A+x;
		// if(yout_1=yout_3&&yout_1!=0)break;
	}
	k0=yout_1;   
	T=A/k0;
	kp=rin/k0;
	ti=1.8*T;
	td=0.167*T;
	ki=kp*ts/ti;
	kd=kp*td/ts;
	printf("kp= %lf\n",kp);
	printf("ki= %lf\n",ki);
	printf("kd= %lf\n",kd);
	printf("num_2= %lf\n",num_2);
    printf("num_3= %lf\n",num_3);
    printf("den_2= %lf\n",den_2);
    printf("den_3= %lf\n",den_3);
	printf("A= %lf\n",A);
 
   

 




}