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"
    
13. 
    
14. u8 RUN_MODE=0;
    
15. u8 Print=0;
    
16. 
    
17. float pitch,roll,yaw;                 //欧拉角
    
18. short aacx,aacy,aacz;                //加速度传感器原始数据
    
19. short gyrox,gyroy,gyroz;        //陀螺仪原始数据
    
20. Pid_struct motor13_parameter,motor24_parameter;  // 电机pid参数
    
21. 
    
22. void NVIC_Config(void);
    
23. 
    
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();   
    
36. 
    
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. }          }         }
    
59. 
    
60. 
    
61. void NVIC_Config(void)
    
62. {
    
63.          
    
64.    
    
65.          NVIC_InitTypeDef NVIC_InitStructure;                                          //复位NVIC寄存器为默认值       
    
66.      NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);           //设置中断优先级组
    
67. 
    
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 );
    
77. 
    
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. }
    
97. 
    

复制代码

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

2023-11-22 19:19 上传

点击文件名下载附件
下载积分: 黑币 -5