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


	//PID參數(shù)初始化 G(s)=m_k*exp(-tao*s)/[(m_t1s+1)(m_t2s+1)]
    double m_t1,m_t2,m_k,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];//PID控制器輸出
     
   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;
  }


  //參數(shù)整定
  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=x+e[k]*k*ts;
		 
		 if(yout_1==yout_3&&yout_1!=0)break;
	}
  //num_3=num_2;
    //A=k*ts*yout_1-x;
    A=x;
	k0=yout_1;   
	T=A/k0;
	kp=rin/k0;
	ti=1.7*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);
    printf("k= %d\n",k);
	printf("yout_1= %lf\n",yout_1);
//	for(int m=0;m<=200;m++)
//	{
//		 printf("yout[%d]= %lf\n",m,yout[m]);
//	}




	double rin2,yout1[500],error2=0;
	error_1=0.0;
   
     error=0.0;
     perror=0.0;
     ierror=0.0;
     derror=0.0;

     yout_1=0;yout_2=0;error=0;
     printf("input 系統(tǒng)輸入 rin2:  rin2=");
     scanf("%lf",&rin2);

    double u_[10];
	int v;
	for(v=0;v<=10;v++)
	{
		u_[v]=0.0;
	}
    for( int q=0;q<=a;q++)
	{
		u[q]=kp*perror+kd*derror+ki*ierror;
		if(u[q]>=10)u[q]=10;
		if(u[q]<=-10)u[q]=-10;
		yout1[q]=-den_2*yout_1-den_3*yout_2+num_2*u_[n]+num_3*u_[n+1];
		 for (int j=n+1;j>0;j--)
         {
             u_[j]=u_[j-1];
         }
		 u_[0]=u[q];
	//	printf("yout1[%d]= %lf\n",q,yout1[q]);
		error2=rin2-yout1[q];
		 
		 yout_2=yout_1;
		 yout_1=yout1[q];
		 perror=error2;
		 ierror=ierror+error2*ts;
		 derror=(error2-error_1)/ts;
		 error_1=error2;
		 
	}
	for(int r=0;r<=280;r++)printf("yout1[%d]= %lf\n",r,yout1[r]);


 




}