电磁组舵机控制的进来看一下

1074347664

不知道有人参考国防科技大学铁军二师的技术报告，小弟略有点不懂   想请教一下，主要是这个位置解算算法有几个值不知道是什么意思

void Getanticipateposition1(void)                //获取小车运行期望位置（用双排X Y进行决策,X用多项式插值）
{
    uint8 i,num;
       
        for(i=99;i>0;i--)
    {
       position1[i]=position1[i-1];
    }
    a=-0.0007*ADx1[0]+0.0013*ADx1[1]-0.0013*ADx1[2]+0.0007*ADx1[3];

    b=0.0067*ADx1[0]-0.0067*ADx1[1]-0.0067*ADx1[2]+0.0067*ADx1[3];
    c=0.0167*ADx1[0]-0.1333*2.0*ADx1[1]+0.1333*2.0*ADx1[2]-0.0167*ADx1[3];
    //a=a*100.00;
    lmax=ADx1[0];                            //取最大值
    num=0;
    for(i=1;i<4;i++)
    {
        if(ADx1[i]>lmax)
        {
          lmax=ADx1[i];
          num=i;
        }
    }
    switch(num)
        {
        case 0: //if(ADx1[0]-ADx1[1]>4)
          if(ADx1[0]-ADx1[1]>4)
                        {
                            gp=(ADx1[1]-132.7)/5.975;
                            position1[0]=gp;
                            if(position1[0]<-15)
                            position1[0]=-15;                
                        }
                        else
                        {
                          p1=(-2.0*b+sqrt(4.0*b*b-12.0*a*c))/(6.0*a);
                           p2=(-2.0*b-sqrt(4.0*b*b-12.0*a*c))/(6.0*a);
                           p3=6.0*p1*a+2.0*b;
                           if(p3<0)
                                  position1[0]=p1;
                           else
                                  position1[0]=p2;
                        }

                        break;

        case 1: p1=(-2.0*b+sqrt(4.0*b*b-12.0*a*c))/(6.0*a);
            p2=(-2.0*b-sqrt(4.0*b*b-12.0*a*c))/(6.0*a);
            p3=6.0*p1*a+2.0*b;
            if(p3<0)
              position1[0]=p1;
            else
              position1[0]=p2;
                        break;

        case 2: p1=(-2.0*b+sqrt(4.0*b*b-12.0*a*c))/(6.0*a);
            p2=(-2.0*b-sqrt(4.0*b*b-12.0*a*c))/(6.0*a);
                        p3=6.0*p1*a+2.0*b;
                        if(p3<0)
                          position1[0]=p1;
                        else
              position1[0]=p2;
                        break;

        case 3: //if(ADx1[3]-ADx1[2]>4)
          if(ADx1[3]-ADx1[2]>4)
                        {
                           gp=(132.7-ADx1[2])/5.975;              //同上
                           position1[0]=gp;
                           if(position1[0]>15)
                                 position1[0]=15;
                        }
                        else
                        {
                           p1=(-2.0*b+sqrt(4.0*b*b-12.0*a*c))/(6.0*a);
                           p2=(-2.0*b-sqrt(4.0*b*b-12.0*a*c))/(6.0*a);
                           p3=6.0*p1*a+2.0*b;
                           if(p3<0)
                                  position1[0]=p1;
                           else
                                  position1[0]=p2;
                        }
                        break;

        }
    psum=ADx1[0]+ADx1[1]+ADx1[2]+ADx1[3];
    if(psum<65)    //这个量也需要重新测定
    {
        if(position1[1]<-5)
            position1[0]=-15;
        else if(position1[1]>5)
            position1[0]=15;
        paopianflag=1;
    }
    else
    {
        paopianflag=0;
    }

        if(position1[0]>15)
                position1[0]=15;
        if(position1[0]<-15)
                position1[0]=-15;
       
}