智能车制作

标题: 电磁组舵机控制的进来看一下 [打印本页]

作者: 1074347664 时间: 2014-7-8 02:57
标题: 电磁组舵机控制的进来看一下
不知道有人参考国防科技大学铁军二师的技术报告，小弟略有点不懂想请教一下，主要是这个位置解算算法有几个值不知道是什么意思

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;

}

欢迎光临智能车制作 (http://dns.znczz.com/)