標(biāo)題: STM32F103風(fēng)力擺系統(tǒng)程序,陀螺儀使用基于DMP的MPU6050可自行增加卡爾曼濾波 [打印本頁]
作者: 超大俠    時(shí)間: 2023-11-21 17:01
標(biāo)題: STM32F103風(fēng)力擺系統(tǒng)程序,陀螺儀使用基于DMP的MPU6050可自行增加卡爾曼濾波
基于STM32F103的風(fēng)力擺系統(tǒng)，陀螺儀使用基于DMP的MPU6050可自行增加卡爾曼濾波

單片機(jī)源程序如下:
  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;                //加速度傳感器原始數(shù)據(jù)
  19. short gyrox,gyroy,gyroz;        //陀螺儀原始數(shù)據(jù)
  20. Pid_struct motor13_parameter,motor24_parameter;  // 電機(jī)pid參數(shù)

  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();                     //延時(shí)函數(shù)初始化       
  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);   // 電機(jī)pid 參數(shù)初始化
  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);   //定時(shí)器初始化
  45.    uprintf(USART1," 系統(tǒng)啟動(dòng)成功!\r\n");
  46.          
  47. while(1)
  48. {               
  49.    //           MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        //得到加速度傳感器數(shù)據(jù)
  50.    //    MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);        //得到陀螺儀數(shù)據(jù)   
  51.   KEY_Scan();
  52.   if(Print==1){   // 發(fā)送曲線到上位機(jī)
  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;                                          //復(fù)位NVIC寄存器為默認(rèn)值       
  66.      NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);           //設(shè)置中斷優(yōu)先級組

  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.               //復(fù)位NVIC寄存器為默認(rèn)值          
  85.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;       
  86.           NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;   //搶占優(yōu)先級2
  87.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;              //響應(yīng)優(yōu)先級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. }

復(fù)制代碼

原理圖: 無
仿真: 無
代碼: MPU6050_DMP_Stm32.7z (289.75 KB, 下載次數(shù): 18)




歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1