4.全方案整合

您当前位置:首页>STM32平衡小车>平衡原理

四、全方案整合


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

1493394307361854副本.jpg

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

        (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输出

        }