速度一直控制不了！求大神指导

来电5566

  
我一直不知道什么原因  控制不下来
void PI_sudu(void)
{
    nLeftSpeed = speed1;
    nRightSpeed = speed2;
    nSpeed1 = (nLeftSpeed + nRightSpeed) / 2;
    nSpeed1= nSpeed1*10/157 ;
   

   EE0 = SpeedSet - nSpeed1; //增量计算

   fP=EE0*Sudu_P;
   fI=EE0*Sudu_I;
   SpeedIntegral+=fI;               
   EE2=EE1;
   EE1=fP+SpeedIntegral;
      
}

void SpeedOutput()
{
float fValue;
fValue=EE1;
  sp=(int)(fValue*(CAR_Count2+1)/20+EE2);
  
   if(sp>MOTOR_OUT_MAX)
       sp=MOTOR_OUT_MAX;
   if(sp<MOTOR_OUT_MIN)
       sp=MOTOR_OUT_MIN;
   
   Left_Speed=Right_Speed=sp;
}

这个是速度函数
void MotorOutput()
{
     nLeft=nSpeed-Left_Speed-Left_Right_Diff;
     nRight=nSpeed-Right_Speed+Left_Right_Diff;
     g_nLeftMotorOut = nLeft;
     g_nRightMotorOut = nRight ;
     if(g_nLeftMotorOut > MOTOR_OUT_MAX)  g_nLeftMotorOut = MOTOR_OUT_MAX;
     if(g_nLeftMotorOut < MOTOR_OUT_MIN)  g_nLeftMotorOut = MOTOR_OUT_MIN;
     if(g_nRightMotorOut > MOTOR_OUT_MAX) g_nRightMotorOut = MOTOR_OUT_MAX;
     if(g_nRightMotorOut < MOTOR_OUT_MIN) g_nRightMotorOut = MOTOR_OUT_MIN;
     MotorSpeedOut();
}
这个是电机输出
  if(QingJiao>0)
      {
        speed1=-speed1;
        speed2=-speed2;
      }
这个是极性的判断  
我看不出是什么问题啊