stm32平衡车源码大放送，基于DMP，卡尔曼滤波，一阶滤波应有尽有，还有

lixiao · 发表于 2019-2-9 17:24:31

本帖最后由 lixiao 于 2019-2-9 17:38 编辑

资料拿走不谢，希望能够帮助到正在做平衡小车的同学
[mw_shl_code=cpp,true]int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
u8 Flag_Target;
int Voltage_Temp,Voltage_Count,Voltage_All;
/**************************************************************************
函数功能：所有的控制代码都在这里面
      5ms定时中断由MPU6050的INT引脚触发
      严格保证采样和数据处理的时间同步
**************************************************************************/
int EXTI9_5_IRQHandler(void)
{
      if(PBin(5)==0)
      {
               EXTI->

R=1<<5;                                                    //清除LINE5上的中断标志位
               Flag_Target=!Flag_Target;
               if(delay_flag==1)
                     {
                              if(++delay_50==10)       delay_50=0,delay_flag=0;                   //给主函数提供50ms的精准延时
                     }
               if(Flag_Target==1)                                                 //5ms读取一次陀螺仪和加速度计的值，更高的采样频率可以改善卡尔曼滤波和互补滤波的效果
                     {
                              Get_Angle(Way_Angle);                                              //===更新姿态
   Voltage_Temp=Get_battery_volt();                                              //=====读取电池电压
                     Voltage_Count++;                                                 //=====平均值计数器
                     Voltage_All+=Voltage_Temp;                                        //=====多次采样累积
                     if(Voltage_Count==100) Voltage=Voltage_All/100,Voltage_All=0,Voltage_Count=0;//=====求平均值
                     return 0;
                     }                                                                //10ms控制一次，为了保证M法测速的时间基准，首先读取编码器数据
                     Encoder_Left=-Read_Encoder(2);                                     //===读取编码器的值，因为两个电机的旋转了180度的，所以对其中一个取反，保证输出极性一致
                     Encoder_Right=Read_Encoder(4);                                     //===读取编码器的值
               Get_Angle(Way_Angle);                                              //===更新姿态
                     Read_Distane();                                                    //===获取超声波测量距离值
               if(Bi_zhang==0)Led_Flash(100);                                     //===LED闪烁;常规模式 1s改变一次指示灯的状态
                     if(Bi_zhang==1)Led_Flash(0);                                     //===LED闪烁;避障模式指示灯常亮
                     Key();                                                             //===扫描按键状态单击双击可以改变小车运行状态
                     Balance_Pwm =balance(Angle_Balance,Gyro_Balance);                //===平衡PID控制
               Velocity_Pwm=velocity(Encoder_Left,Encoder_Right);                //===速度环PID控制       记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
         Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn);          //===转向环PID控制
               Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm;                         //===计算左轮电机最终PWM
               Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm;                         //===计算右轮电机最终PWM
               Xianfu_Pwm();                                                    //===PWM限幅
                     if(Pick_Up(Acceleration_Z,Angle_Balance,Encoder_Left,Encoder_Right))//===检查是否小车被那起
                     Flag_Stop=1;                                                             //===如果被拿起就关闭电机
                     if(Put_Down(Angle_Balance,Encoder_Left,Encoder_Right))             //===检查是否小车被放下
                     Flag_Stop=0;                                                             //===如果被放下就启动电机
   if(Turn_Off(Angle_Balance,Voltage)==0)                            //===如果不存在异常
                     Set_Pwm(Moto1,Moto2);                                              //===赋值给PWM寄存器
      }
      return 0;
} [/mw_shl_code]

[mw_shl_code=cpp,true]u8 Way_Angle=1;                            //获取角度的算法，1：四元数  2：卡尔曼  3：互补滤波
u8 Flag_Qian,Flag_Hou,Flag_Left,Flag_Right,Flag_sudu=2; //蓝牙遥控相关的变量
u8 Flag_Stop=1,Flag_Show=0;                //停止标志位和显示标志位默认停止显示打开
int Encoder_Left,Encoder_Right;          //左右编码器的脉冲计数
int Moto1,Moto2;                         //电机PWM变量应是Motor的向Moto致敬
int Temperature;                         //显示温度
int Voltage;                               //电池电压采样相关的变量
float Angle_Balance,Gyro_Balance,Gyro_Turn; //平衡倾角平衡陀螺仪转向陀螺仪
float Show_Data_Mb;                      //全局显示变量，用于显示需要查看的数据
u32 Distance;                            //超声波测距
u8 delay_50,delay_flag,Bi_zhang=0,PID_Send,Flash_Send;  //延时和调参等变量
float Acceleration_Z;                      //Z轴加速度计
float Balance_Kp=300,Balance_Kd=1,Velocity_Kp=80,Velocity_Ki=0.4;//PID参数
u16 PID_Parameter[10],Flash_Parameter[10];  //Flash相关数组
int main(void)
{
      Stm32_Clock_Init(9);          //=====系统时钟设置
      delay_init(72);                //=====延时初始化
      JTAG_Set(JTAG_SWD_DISABLE);    //=====关闭JTAG接口
      JTAG_Set(SWD_ENABLE);          //=====打开SWD接口可以利用主板的SWD接口调试
      LED_Init();                   //=====初始化与 LED 连接的硬件接口
      KEY_Init();                   //=====按键初始化
      OLED_Init();                   //=====OLED初始化
      uart_init(72,128000);          //=====初始化串口1
  uart3_init(36,9600);          //=====串口3初始化
  MiniBalance_PWM_Init(7199,0); //=====初始化PWM 10KHZ，用于驱动电机如需初始化电调接口改为MiniBalance_PWM_Init(9999,35) 200HZ
      Encoder_Init_TIM2();          //=====编码器接口
      Encoder_Init_TIM4();          //=====初始化编码器2
      Adc_Init();                   //=====adc初始化
      IIC_Init();                   //=====模拟IIC初始化
  MPU6050_initialize();          //=====MPU6050初始化
  DMP_Init();                   //=====初始化DMP
      TIM3_Cap_Init(0XFFFF,72-1);          //=====超声波初始化
      EXTI_Init();                   //=====MPU6050 5ms定时中断初始化
      while(1)
            {
                                    if(Flag_Show==0)       //使用MiniBalance APP和OLED显示屏
                                    {
                                             APP_Show();
                                             oled_show();          //===显示屏打开
                                    }
                                    else                   //使用MiniBalance上位机上位机使用的时候需要严格的时序，故此时关闭app监控部分和OLED显示屏
                                    {
                                 DataScope();       //开启MiniBalance上位机
                                    }
                              delay_flag=1;
                                    delay_50=0;
                                    while(delay_flag);          //通过MPU6050的INT中断实现的50ms精准延时
            }
}
[/mw_shl_code]