求助，各位看看这128程序有啥问题？

RUYING20000

这是我要调直立的一段程序，车是直立起来了，但是回复力很小，稍用劲戳一下就倒了，还有一点小抖动，怎么调参数都没法解决这个问题？请各位看看这程序有啥问题。。
if (ANG > AD_AVERIGE[1])            
    {            
        DTY1=((ANG - AD_AVERIGE[1]) * PID1_P + (AD_AVERIGE[0]-GYRO) / PID1_D);
        DTY2=((ANG - AD_AVERIGE[1]) * PID2_P + (AD_AVERIGE[0]-GYRO) / PID2_D);     
        if(DTY1>=DTYMAX) DTY1=DTYMAX;
        if(DTY2>=DTYMAX) DTY2=DTYMAX;  
        PWM_DIR0=1;            
        PWM_DIR1=1;
        PWMDTY3=(byte)DTY1- SpeedControlOut;
        PWMDTY7=(byte)DTY2- SpeedControlOut;
        
        PWMPOL=0X00;      //输出极性选择 0起始为低电平 1起始为高电平        
    }   
    //Delay_us(5);        
    /**********************反转***************/
    else if (ANG< AD_AVERIGE[1])              
    {
        DTY1=((  AD_AVERIGE[1] - ANG ) * PID1_P + (GYRO -  AD_AVERIGE[0]) / PID1_D );
        DTY2=((  AD_AVERIGE[1] - ANG ) * PID2_P + (GYRO -  AD_AVERIGE[0]) / PID2_D );                        
        if(DTY1>=DTYMAX) DTY1=DTYMAX;   
        if(DTY2>=DTYMAX) DTY2=DTYMAX;         
        PWM_DIR0=0;
        PWM_DIR1=0;
        PWMDTY3=(byte)DTY1;
        PWMDTY7=(byte)DTY2;
        PWMPOL=0Xff;               
    }