哪位大神给我看下这个06届飞思卡尔杭电的程序,pd控制舵机的部分

mmelodious

麻烦哪位大神给我看下后解释下,附件里的技术报告中舵机控制部分的变量error1怎样由传感器数据处理得来,代码如下,菜鸟再此先谢谢了!
void turn_DuoJ()
{
     char i;
     adzhi=New_value[0]+New_value[1]+New_value[2];
     
     if(New_value[1]>400)
     {
       if(error1>100)
       error1=100;
       else if(error1<-100)
       error1=-100;
     }
     
    if(New_value[1]>350)
     {
       if(error1>200)
       error1=200;
       else if(error1<-200)
       error1=-200;
     }
     
   for(i=4;i>0;i--)
      E1=E1[i-1];
     
   E1[0]=(int)error1;
   D1=KP1*E1[0]-KP1*E1[1];
   D2=KP2*E1[0]-KP2*E1[1];
   D3=KP3*E1[0]-KP3*E1[1];
   if(D1>1000)
     D1=1000;
   else if(D1<-1000)
     D1=-1000;
   if(D2>1000)
     D2=1000;
   else if(D2<-1000)
     D2=-1000;
   if(D3>1000)
     D3=1000;
   else if(D3<-1000)
     D3=-1000;
   
  if(error1<=fanwei2&&error1>=-fanwei2) //fanwei2<fanwei
  {
     KP_temp=KP3;
     KD_temp=KDc;
     DD=D3;
  }
  else if(error1<=fanwei&&error1>=-fanwei)
  {
     KP_temp=KP1;
     KD_temp=KDa;
     DD=D1;
  }
  else
  {
     KP_temp=KP2;
     KD_temp=KDb;
     DD=D2;
  }
  
  
  if(DD>16000/KD_temp&&DD>0)
     DD=16000/KD_temp;
  else if(DD<-16000/KD_temp&&DD<0)
     DD=-16000/KD_temp;
  
  if(error1>26000/KP_temp&&error1>0)
     error1=26000/KP_temp;
  else if(error1<-26000/KP_temp&&error1<0)
     error1=-26000/KP_temp;
  if(lock==1&&New_value[2]>80)
  {
    if(DS>0&&New_value[2]>New_value[0]&&New_value[2]>90)
         lock=0;
    else if(DS<0&&New_value[0]>New_value[2]&&New_value[0]>90)
          lock=0;
  }

j715473086

这是传说中的分段PD吗

haozi110

用现在的减去以前的

j715473086

研究了一下 删掉的东西太多 没借鉴意义