/******************************************************************************/
/* 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)三区进入二区时的转速(°/s),w(tf)二区进入一区时的转速
//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;
//调节原则:随着|Δe|降低减小Kp、增加Kd、h2(Ki);
if(error[1]>=16300) //砰砰区
{ h1=2000; h2=0; h3=0;
}
else if(error[1]<=-16300)
{ h1=-2000; h2=0; 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;
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区
{ /*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;
//一组优化的数值k1=2、k2=4.5、k3=14.5,k4=0.02
return (control);//返回当前时刻的控制量。
}