摄像头转向算法

1023968138

大家直立车是怎么用的转向算法的，加权，直立车怎么加啊？加的云里雾里的

小朴少-

#define DERICTION_CONTROL_PERIOD 1
int    error=0;
float  D_P=0;
float  D_D=0;
float  ERROR1=0;
float  fvalue_new=0;
float  fvalue_old=0;
float  Middle,Midlle;
float  g_fDirectionControlOut;
int    g_nDirectionControlPeriod;
float  g_fDirectionControolOld,g_fDirectionControolNew;
void Direction_Control(void)
{
  float fvalue,fDvalue;
for(i=IMAGE_H-1;i>1;i--)
{  
                 fvalue=center[i]-0;
  //ImageBuffer1[i][center[i]] = 100;  
}
  if(fvalue<0)  //zuo
   {
    D_P=0.25 ;//0
    D_D=1.8;//0
   }
  else          //you
   {
    D_P=0.10;//0
    D_D=1.8;// 0
   }
  g_fDirectionControolOld=g_fDirectionControolNew;
  ERROR1=fvalue - fvalue_old;
  if(ERROR1>15)   ERROR1 = 15;
  if(ERROR1<-15)  ERROR1 =-15;
  fDvalue=fvalue*D_P+ERROR1*D_D;
  fvalue_old=fvalue;
  g_fDirectionControolNew=fDvalue;
}
//////////////////////////////////////////////////////////////////////////////////////
void DirectinControlOutput(void)
{
  float fValue;
  fValue=g_fDirectionControolNew-g_fDirectionControolOld;
  MotorTurnPWM= fValue*(0+1)/DERICTION_CONTROL_PERIOD+g_fDirectionControolOld;
//printf("MotorTurnPWM:%d\r\n",MotorTurnPWM);
}
#endif
#if 1
void Send_Img(void)
{  
  uint8_t i,j;
        printf("%c", 0xFF);
        for(i=0;i<IMAGE_H;i++)
  {
          for(j=0;j<IMAGE_W;j++)
                printf("%c",ImageBuffer1[i][j]);
  }
}
#endif