智能车制作

标题: 请各位大神指点迷津谢谢了 [打印本页]

作者: xiaojiong 时间: 2012-11-16 16:01
标题: 请各位大神指点迷津谢谢了
请大神指点  这样为什么没能实现直立的闭环控制

/*//// 直立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);
}

欢迎光临智能车制作 (http://dns.znczz.com/)