本帖最后由 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]