四、全方案整合

        1.通过上面介绍，将车模直立行走主要的控制算法集中起来，如图

        为了实现小车直立行走，需要采集如下信号：

        （1）小车倾角速度陀螺仪信号，获得小车的倾角和角速度。

        （2） 重力加速度信号

        （z轴信号），补偿陀螺仪的漂移。该信号可以省略，有速度控制替代。

        （3） 小车电机转速脉冲信号，获得小车运动速度，进行速度控制。

        2.在小车控制中的直立和速度控制两个环节中，分别使用了角度PD控制和速度PI控制，这两种控制算法的输出量最终通过叠加通过电机运动来完成。

        （1）小车直立控制：使用小车倾角的PD（比例、微分）控制；

        void AngleControl(void) 

        {

        BST_fCarAngle = Roll - CAR_ZERO_ANGLE;//DMP ROLL滚动方向角度与预设小车倾斜角度值的差得出角度   

        BST_fAngleControlOut =  BST_fCarAngle * BST_fCarAngle_P + gyro[0] * BST_fCarAngle_D ;  //角度PD控制   

        }

        （2）小车速度控制：使用PI（比例、积分）控制；

        void SpeedControl(void)

        {

        BST_fCarSpeed = (BST_s32LeftMotorPulseSigma  + BST_s32RightMotorPulseSigma );  

        //左右电机脉冲数平均值作为小车当前车速

        BST_s32LeftMotorPulseSigma =BST_s32RightMotorPulseSigma = 0;  

        //全局变量 注意及时清零

        BST_fCarSpeedOld *= 0.7;

        BST_fCarSpeedOld +=BST_fCarSpeed*0.3;

        BST_fCarPosition += BST_fCarSpeedOld; //路程  即速度积分

        BST_fCarPosition += BST_fBluetoothSpeed;   //融合蓝牙给定速度

        BST_fCarPosition +=fchaoshengbo;   //融合超声波给定速度

        if(stopflag==1)

        {

             BST_fCarPosition=0;

        }//悬停以后的位置

        //积分上限设限//

        f((s32)BST_fCarPosition > CAR_POSITION_MAX)    BST_fCarPosition = CAR_POSITION_MAX;

        if((s32)BST_fCarPosition < CAR_POSITION_MIN)    BST_fCarPosition = CAR_POSITION_MIN;          

        BST_fSpeedControlOutNew = (BST_fCarSpeedOld -CAR_SPEED_SET ) * BST_fCarSpeed_P + (BST_fCarPosition - CAR_POSITION_SET ) * BST_fCarSpeed_I; //速度PI算法 速度*P +位移*I=速度PWM输出

        }