六、程序（只给出一部分内容）

 （1） 时序总算法  

    void SysTick_Handler(void) //5ms定时器

    {  

        BST_u8MainEventCount++;   //总循环计数值

        BST_u8trig++;

        BST_u8SpeedControlCount++;  //小车速度控制调用计数值GetMotorPulse();//脉冲计算函数BST_u8SpeedControlPeriod++;

        BST_u8DirectionControlPeriod++;   //转向平滑输出计算比例值

        BST_u8DirectionControlCount++;

        AngleControl();  //角度PD控制PWNM输出

        MotorOutput();  //小车总PWM输出  

        if(BST_u8trig>=2)

        {

            UltrasonicWave_StartMeasure();   //调用超声波发送程序 给Trig脚 <10us 高电平chaoshengbo();       //计算超声波测距距离

            BST_u8trig=0;

        }

        if(BST_u8SpeedControlCount>=8)       //当计数值8时，即总系统运行40ms时候(每10个角度PWM输出中融入1个速度PWM输出，这样能保持速度PID输出不干扰角度PID输出，从而影响小车平衡)

        {

             SpeedControl();                     //车模速度控制函数   每40ms调用一次

             BST_u8SpeedControlCount=0;  //小车速度控制调用计数值清零

             BST_u8SpeedControlPeriod=0;  //平滑输出比例值清零

         }    

    }