调节速度P和I会对车子有影响，但车还是会往一个方向走，求大神帮忙，

justlive

/********************************************/
//函数 : 数度控制
//名称 : SpeedControl();
//
/*********************************************/
void SpeedControl(void)
{
     float fP;
     float fI;
     int fDelta1=0;
     int fDelta;
  //   CarSpeed=(Left+Right)/2;
     CarSpeed = Left;
     Left=Right=0;
  //   CarSpeed*=2;
     fDelta=0;
     
    fDelta=fDelta1-CarSpeed;
     SPEED_P =0.5;
     fP=fDelta*SPEED_P;
     SPEED_I=0;
     fI=fDelta*SPEED_I;
     Speedlast+=fI;
     if(Speedlast<-50){
        Speedlast=-50;
     }
     if(Speedlast>50){
        Speedlast=50;
     }
     SpeedOld=SpeedNew;
     SpeedNew=fP+Speedlast;
     LCD_write_english_string(0,0,"SO");
     LCD_Write_Num(20,0,SpeedOld,5);  
     LCD_write_english_string(0,4,"SN");
     LCD_Write_Num(20,4,SpeedNew,5);   
}
/*****************************************/
//函数 : 速度平滑输出
//名称 : Speedput(void);
//
/******************************************/
void Speedput(void)
{
     float fValue;
     fValue=SpeedNew-SpeedOld;
     SpeedOut=fValue*(SpeedPeriod+1)/100+SpeedOld;
     LCD_write_english_string(0,2,"SO");
     LCD_Write_Num(20,2,SpeedOut,5);
}
/**********************************************/
//函数 : 电机驱动函数
//名称 : void SetMotorVoltage(float fLeftVoltage,float fRightVoltage)
//
/**********************************************/
void SetMotorVoltage(float fLeftVoltage,float fRightVoltage)
{   
    if(fLeftVoltage>0){
      if(fLeftVoltage>255){fLeftVoltage=255;}
      PWMDTY67=(int)fLeftVoltage;
      PWMDTY45=0;
    }
   else if(fLeftVoltage<0){
      fLeftVoltage=-fLeftVoltage;
      if(fLeftVoltage>255){fLeftVoltage=255;}
      PWMDTY67=0;
      PWMDTY45=(int)fLeftVoltage;
      }
  if(fRightVoltage>0){
      if(fRightVoltage>255){fRightVoltage=255;}
      PWMDTY23=(int)fRightVoltage;
      PWMDTY01=0;
      }
   else if(fRightVoltage<0){
      fRightVoltage=-fRightVoltage;
      if(fRightVoltage>255){fRightVoltage=255;}
      PWMDTY23=0;
      PWMDTY01=(int)fRightVoltage;
   }
}
/**********************************************/
// 函数 : 电机输出函数
// 名称 : Mtorput();
//
/**********************************************/
void Motorput(void)
{
     float fLeft,fRight;
     if(AngleOut>0){
        fLeft=AngleOut+SpeedOut/*-DirectionOut*/;
        fRight=AngleOut+SpeedOut/*+DirectionOut*/;      
              }
    if(AngleOut<0){
        fLeft=AngleOut-SpeedOut/*-DirectionOut*/;
        fRight=AngleOut-SpeedOut/*+DirectionOut*/;
              }
    LeftOut=fLeft;
    RightOut=fRight;
    SetMotorVoltage(LeftOut,RightOut);
    LCD_write_english_string(0,1,"LO");
    LCD_Write_Num(20,1,LeftOut,5);
  }
/***********************************************/
// 函数 : 主函数
// 名称 : main();
//
/***********************************************/   
void main(void)
{
      DisableInterrupts;
      PLL_INIT();
      init_pwm();
    //  PORT_INIT();
      ATD_INIT();
      PIT_INIT();
      PACNT_INIT();
      PORT_Init();
      LCD_clear();
       LCD_init();
      EnableInterrupts;
      for(;;)
      {
      
      }
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED   
interrupt VectorNumber_Vpit0 void PIT_ISR(void)
{
    PITTF_PTF0=1;
    SpeedPeriod++;
    Speedput();
//   DirectionPeriod++;
//   Directionput();
    count = count+1;
    if(count==1){       //脉冲计数
       TIM1_init();
     //  LeftPulse=PACNT;
        LCD_write_english_string(0,5,"LP");
       LCD_Write_Num(20,5,LeftPulse,5);
    }
    else if(count==2){  //AD采集
      
      // TIM2_init();
       ATD_capture();
    }
    else if(count==3){   //角度计算与直立
    //   RightPulse=PACNT;
       AngleCalculate();
       AngleControl();
       Motorput();
    }
    else if(count==4){   //速度控制
         SpeedCount++;
         Left+=LeftPulse;
      //   Right+=RightPulse;
         Right+=LeftPulse;
         if(SpeedCount>=10){
            SpeedControl();
            SpeedCount=0;
            SpeedPeriod=0;
         }
    }
    else if(count==5){
      
        count=0;
    }
}
#pragma CODE_SEG DEFAULT