请各位大神指点迷津 谢谢了

xiaojiong

请大神指点  这样为什么没能实现直立的闭环控制

/*//// 直立ad采样/////////////////*/
void AD_CaiY(void){
  gyro=(float)(ReadAD(0))*4240/1024;
  gyro=(gyro-gyro_mid)/6.7;
  gjiasu= (float)(ReadAD(1))*4240/1024;
  gjiasu=(gjiasu-gjiasu_mid)/800;
  gjiasu=asin(gjiasu);
  gjiasu*=57.3248;
  complement_filter(gyro,gjiasu);
}
/*/////角度融合///////////*/
void complement_filter(float angle_m_cf,float gyro_m_cf)
{
   angle=(angle+gyro_m_cf*dt)*0.995+angle_m_cf*0.005;
}
void PWM_Cal(void)
{   
    PWMOUT=Kp_angle*(angle-angleset)-Kd_angle*gyro;
    if(angle>5)
     {
      PWMOUT=5000;                      //超过中心值正负7度就停车
     }
     else if(angle<-25)
     {
      PWMOUT=5000;
     }
   
    if(PWMOUT>10000)PWMOUT=10000;
    else if(PWMOUT<0)PWMOUT=0;    //对所有环数据融合后进行限幅
    motor(PWMOUT);                   //将输出给电机
    motor1(PWMOUT);
}