求教 国防科大舵机非线性控制算法

liuxiaoxiong

看是看懂了，但是里面的系数d e  f 不知道怎么调，有没有谁用了这个算法的，求教

void Steercontrol(void)                  //PD控制或者非线性控制
{
float pwm;
uint steerpwm;
float positionerror;
if(positiony<20)
xianzhiflag=0.5;
else if(positiony<30)       //若上限为40，则转折点为100  若上限为30，则转折点为753
无挖潜
xianzhiflag=0.05*positiony-0.5;
else
xianzhiflag=1;
if(psum<75)
xianzhiflag=1;
positionerror=position1[0]-position1[1];
if(position1[0]<0)
kpc=d*position1[0]*position1[0]-e*position1[0]+f;
else
kpc=d*position1[0]*position1[0]+e*position1[0]+f;
pwm=(kpc*position1[0]+kdc*positionerror)*xianzhiflag;
steerpwm=csteermid+(int)pwm;   
if(steerpwm<csteerleft)
PWMDTY67=csteerleft;
else if(steerpwm>csteerright)
错误！未找到引用源。
第  XVII  页
{
PWMDTY67=csteerright;
}
else
{
PWMDTY67=steerpwm;
}      
}