调直立时候的电机输出函数要怎么写

静。。子

这样写感觉车子都不动
void AngleControl()
{
       float fValue;
       int i;
       fValue=(CAR_ANGLE_SET-g_fCarAngle)*ANGLE_CONTROL_P +(CAR_ANGLE_SPEED_SET-g_fGyroscopeAngleSpeed)*ANGLE_CONTROL_D;

       g_fAngleControlOut=fValue;

}
void MotorOutput()
{
    PWMDTY01=g_fAngleControlOut;
    PWMDTY23=0;
    PWMDTY45=g_fAngleControlOut;
    PWMDTY67=0;
}