/******************************************************************************/
/*  THIS PROGRAM IS USED TO TEST YOUR CONTROL METHOD'S PERFORMANCE.           */
/*       Please input your program in MySm() then run it.In your program,you  */
/*   can use the variables passed from the main program.                      */
/******************************************************************************/
#include <vcl.h>
#include <math.h>
#pragma hdrstop

USEFORM("E:\servo\Unit1.cpp", Form1);
//---------------------------------------------------------------------------
#define MAX_ERROR_NUM  1000
#define MAX_IN_NUM 64
extern "C" __declspec(dllexport) __stdcall double
       MySm(unsigned short int *,unsigned short int *,short int *,double,
            double,float,float,float,float,float,float,float,float,float,
            float,float,float,float,float,float,float,float,float,float,float);

int WINAPI DllEntryPoint(HINSTANCE hinst, unsigned long reason, void*)
{
   return 1;
}
/******************************************************************************/
/* Variables Declaration:                                                     */
/*   uiInSig[64] is input signal array,include the current value uiInSig[63]    */
/*              and the last three time values,its unit is DELTA.             */
/*   uiOutSig[4] is output signal array,include the current value uiOutSig[3] */
/*              and the last three time values,its unit is DELTA.             */
/*   iError[MAX_ERROR_NUM] is the error array,and iError[MAX_ERROR_NUM] is    */
/*              the current value,its unit is DELTA.                          */
/*   dIntegral is the integral value of control method,its unit is DELTA.     */
/*   k1 to k20 is the constant values inputed in parameter-set-window.        */
/******************************************************************************/
/*
double __stdcall MySm(unsigned short int uiInSig[64],unsigned short int uiOutSig[4],
                      short int iError[MAX_ERROR_NUM],double dSampleTime,double dIntegral,
                      float k1,float k2,float k3,float k4,float k5,float k6,float k7,
                      float k8,float k9,float k10,float k11,float k12,float k13,
                      float k14,float k15,float k16,float k17,float k18,float k19,
                      float k20)
{
*/
#define c_max 1000
//w(tf)^2-w(t0)^2=2*a_max(theta(tf)-theta(0))
//w(t0)三區(qū)進入二區(qū)時的轉速(°/s)，w(tf)二區(qū)進入一區(qū)時的轉速
//a_max等角加速度(°/s^2)
#define A 10
#define a_max_up ((-9)*(-9)-(-127.7)*(-127.7))
#define a_max_dn (2*(0.55-90))
#define Ts	(5*10^(-3))
#define deta_per_second (Ts)
double __stdcall MySm(unsigned short int INsig[64],unsigned short int OUTsig[4],short int Error[1000],
                      double iTime,double h2,float k1,float k2,float k3,float k4,float k5,float k6,
                      float k7, float k8,float k9,float k10,float k11,float k12,float k13,
                      float k14,float k15,float k16,float k17,float k18,float k19,float k20)

 {
   short int error[2];
   double Kp,Kd,control,w,h1,h3,h4,h5,h6,INch,a_max;
   double kptemp,kitemp;
    a_max=a_max_up/a_max_dn;
   INch=INsig[63]-INsig[62];
   error[1]=Error[MAX_ERROR_NUM-1];
   error[0]=Error[MAX_ERROR_NUM-2];

   h4=(short int)(INsig[63]-INsig[62]);    //前饋
   h5=h4-(short int)(INsig[1]-INsig[0]);

   //w=sqrt(a_max*abs(error[1])*0.005493+4)*182;

   kitemp=fabs(error[1])/200;
   //調節(jié)原則：隨著|Δe|降低減小Kp、增加Kd、h2(Ki);
   if(error[1]>=16300)			//砰砰區(qū)
       { h1=2000;	h2=0;	h3=0;
       }
   else if(error[1]<=-16300)
       { h1=-2000;     h2=0;   h3=0;
       }
   else if(error[1]>=100)		//勻減速區(qū)
       {
        w=sqrt(2*a_max*abs(error[1])*0.005493+127.7*127.7-2*a_max*90);//175.43);
        h1=w*14.5;
        h2=0;
        h6=300*(w*0.005+(error[1]-error[0])*0.005493);
        h3=0;
       }
   else if(error[1]<=-100)
       {
        w=sqrt(2*a_max*abs(error[1])*0.005493+127.7*127.7-2*a_max*90);//175.43);
        h1=-w*14.5;
        h2=0;
        h3=0;
        h6=300*(-w*0.005+(error[1]-error[0])*0.005493);
       }
     else if(error[1]>=40)				//PID區(qū)
       { /*h1=error[1]*0.01*k1;*/         h3=(error[1]-error[0])*2.1;         h2+=0.1*kitemp*error[1];
        //if(h1<-A)h1=-A;
        //if(h1>A)h1=A;
       }
    else if(error[1]<=-40)
       { /*h1=error[1]*0.01*k1;*/         h3=(error[1]-error[0])*2.1;         h2+=0.1*kitemp*error[1];
        //if(h1<-A)h1=-A;
        //if(h1>A)h1=A;
       }
    else  if(error[1]>=20)
       { h1=error[1]*0.1*k1;         h3=(error[1]-error[0])*2.2;         h2+=0.4*kitemp*error[1];
        if(h1<-A)h1=-A;
        if(h1>A)h1=A;
       }
     else if(error[1]<=-20)
       { h1=error[1]*0.1*k1;         h3=(error[1]-error[0])*2.2;         h2+=0.4*kitemp*error[1];
        if(h1<-A)h1=-A;
        if(h1>A)h1=A;
       }
     else  if(error[1]>=10)
       { h1=error[1]*0.3*k1;         h3=(error[1]-error[0])*2.3;         h2+=0.6*kitemp*error[1];
        if(h1<-A)h1=-A;
        if(h1>A)h1=A;
       }
     else if(error[1]<=-10)
       { h1=error[1]*0.3*k1;         h3=(error[1]-error[0])*2.3;         h2+=0.6*kitemp*error[1];
        if(h1<-A)h1=-A;
        if(h1>A)h1=A;
       }
     else if(error[1]>=5)
       { h1=error[1]*0.6*k1;         h3=(error[1]-error[0])*2.4;         h2+=0.8*kitemp*error[1];
        if(h1<-A)h1=-A;
        if(h1>A)h1=A;
       }
     else if(error[1]<=-5)
       { h1=error[1]*0.6*k1;         h3=(error[1]-error[0])*2.4;         h2+=0.8*kitemp*error[1];
        if(h1<-A)h1=-A;
        if(h1>A)h1=A;
       }
      else
       { h1=error[1]*k1;         h3=(error[1]-error[0])*2.5;         h2+=kitemp*error[1];
        if(h1<-A)h1=-A;
        if(h1>A)h1=A;
       }
 
   control=h1+k4*h2+k2*h3+16*h4+3.8*h5-15;


      //一組優(yōu)化的數(shù)值k1=2、k2=4.5、k3=14.5,k4=0.02
   return (control);//返回當前時刻的控制量。
}