6.程序

您当前位置: 首页 > Arduino平衡小车 > PID算法平衡原理

六、程序(只给出一部分内容)

 (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;  //平滑输出比例值清零

         }    

    }