标题: STM32F103风力摆系统程序,陀螺仪使用基于DMP的MPU6050可自行增加卡尔曼滤波 [打印本页]
作者: 超大侠    时间: 2023-11-21 17:01
标题: STM32F103风力摆系统程序,陀螺仪使用基于DMP的MPU6050可自行增加卡尔曼滤波
基于STM32F103的风力摆系统，陀螺仪使用基于DMP的MPU6050可自行增加卡尔曼滤波

单片机源程序如下:
  1. #include "sys.h"       
  2. #include "delay.h"       
  3. #include "key.h"   
  4. #include "mpu6050.h"  
  5. #include "inv_mpu.h"
  6. #include "inv_mpu_dmp_motion_driver.h"
  7. #include "USART2.h"
  8. #include "pwm.h"
  9. #include "PID.h"
  10. #include "TIM.h"
  11. #include "KEY.h"
  12. #include "outputdata.h"

  14. u8 RUN_MODE=0;
  15. u8 Print=0;

  17. float pitch,roll,yaw;                 //欧拉角
  18. short aacx,aacy,aacz;                //加速度传感器原始数据
  19. short gyrox,gyroy,gyroz;        //陀螺仪原始数据
  20. Pid_struct motor13_parameter,motor24_parameter;  // 电机pid参数

  22. void NVIC_Config(void);

  24. int main(void)
  25. {
  26.           short temp;                                        //温度            
  27.           
  28.                 SystemInit();
  29.                 NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
  30.     NVIC_Config();
  31.            delay_init();                     //延时函数初始化       
  32.     PWM_Init(3000,0);    //0分频。PWM频率=72000/(4000*1)=14Khz
  33.     PWM_Init_2(3000,0);   
  34.     USART1_Config();   
  35.     KEY_Init();   

  37.    Speed_PIDInit(&motor13_parameter,30,0.2,100);   // 电机pid 参数初始化
  38.          Speed_PIDInit(&motor24_parameter,60,0.2,200);
  39.        
  40.          MPU_Init();               
  41.    delay_ms(500);   
  42.    while(mpu_dmp_init());   //初始化MPU6050_DMP
  43.      
  44.    TIM4_Init(10000-1,71);   //定时器初始化
  45.    uprintf(USART1," 系统启动成功!\r\n");
  46.          
  47. while(1)
  48. {               
  49.    //           MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        //得到加速度传感器数据
  50.    //    MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);        //得到陀螺仪数据   
  51.   KEY_Scan();
  52.   if(Print==1){   // 发送曲线到上位机
  53.    
  54.      OutData[0]=pitch;
  55.      OutData[1]=roll;
  56.      OutData[2]= motor13_parameter.out_duty/100;         
  57.      OutPut_Data();
  58. }          }         }


  61. void NVIC_Config(void)
  62. {
  63.          
  64.    
  65.          NVIC_InitTypeDef NVIC_InitStructure;                                          //复位NVIC寄存器为默认值       
  66.      NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);           //设置中断优先级组

  68.         RCC_APB2PeriphClockCmd(   RCC_APB2Periph_GPIOA
  69.                                                         | RCC_APB2Periph_GPIOB
  70.                                                             | RCC_APB2Periph_GPIOC
  71.          //   | RCC_APB2Periph_GPIOD
  72.                                         //        | RCC_APB2Periph_AFIO
  73.                                                         | RCC_APB2Periph_TIM1
  74.                                   //        | RCC_APB2Periph_ADC1
  75.               | RCC_APB2Periph_USART1
  76.                                                         , ENABLE );

  78.         RCC_APB1PeriphClockCmd( RCC_APB1Periph_TIM3
  79.                                                         | RCC_APB1Periph_TIM4
  80.               | RCC_APB1Periph_TIM2
  81.                                                         , ENABLE);
  82.       
  83.    
  84.               //复位NVIC寄存器为默认值          
  85.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;       
  86.           NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;   //抢占优先级2
  87.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;              //响应优先级0
  88.           NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;                   
  89.           NVIC_Init(&NVIC_InitStructure);
  90.    
  91.     NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
  92.     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority =0;
  93.                 NVIC_InitStructure.NVIC_IRQChannelSubPriority =0;
  94.     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  95.     NVIC_Init(&NVIC_InitStructure);
  96. }

复制代码

原理图: 无
仿真: 无
代码: MPU6050_DMP_Stm32.7z (289.75 KB, 下载次数: 18)




欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1