关于直立的一个问题

Garic

本人纯新手..单纯想让车站起来而已..

其实站起来的大概步骤是不是这样的:

1.检测出加速度Z的零偏移AD值,和陀螺仪的零便宜AD值..

2.然后带入反三角函数算出角度
  例如: acc=Z_zhou-1440;
   acc=acc/800;
   if(acc>1)
   acc=1;
   else if(acc<-1)
   acc=-1;
   acc=180/3.1415*asin(acc);
   w=(Tuo_L-1136)/0.67;

3.然后带入 Kalman_Filter(acc,w);滤波的子函数里..然后调整static float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,  dt=0.01 ; 这4位变量.

4.然后滤波整合出来的角度带入PWM=K_angle*angle_+K_angle_dot*angle_dot_;再通过PD控制就行了..

请问是这样吗?