標(biāo)題: mpu6050 dmp 歐拉角串口輸出 STM32源程序 [打印本頁]
作者: 李麗麗lili    時間: 2019-4-27 14:44
標(biāo)題: mpu6050 dmp 歐拉角串口輸出 STM32源程序
歐拉角輸出，pitch roll  yaw很精準(zhǔn)

單片機(jī)源程序如下:
  1. #include "sys.h"
  2. #include "usart.h"               
  3. #include "delay.h"
  4. #include "exti.h"
  5. #include "tim.h"
  6. #include "dianji.h"
  7. #include "led.h"
  8. #include "color.h"
  9. #include "exti.h"
  10. #include "duojipwm.h"
  11. #include "huidu.h"
  12. #include "mpu6050.h"
  13. #include "inv_mpu.h"
  14. #include "inv_mpu_dmp_motion_driver.h"
  15. #include "mpuhanshu.h"


  18. #define EXTI_H4_OFF EXTI->IMR&=~(1<<8)
  19. #define EXTI_H6_OFF EXTI->IMR&=~(1<<13)
  20. #define EXTI_Z1_OFF EXTI->IMR&=~(1<<4)
  21. #define EXTI_H4_ON EXTI->IMR|=1<<8
  22. #define EXTI_H6_ON EXTI->IMR|=1<<13
  23. #define EXTI_Z1_ON EXTI->IMR|=1<<4
  24. #define a 95
  25. #define b 170
  26. #define c 60
  27. #define d 240
  28. #define e 0
  29. int dd[22]={70,70,//直行
  30.                  75,65,//右一微轉(zhuǎn)
  31.                  80,60,//右二微轉(zhuǎn)
  32.                  90,50,//右三微轉(zhuǎn)
  33.                  80,30,//右四微轉(zhuǎn)
  34.                  80,20,//右五微轉(zhuǎn)
  35.                  65,75,//左一微轉(zhuǎn)
  36.                  65,80,//左二微轉(zhuǎn)
  37.                  50,90,//左三微轉(zhuǎn)
  38.                  30,80,//左四微轉(zhuǎn)
  39.                  20,80};//左五微轉(zhuǎn)
  40. extern u16 zuoma;
  41. extern u16 youma;
  42. extern s16 zuospeed;
  43. extern s16 youspeed;
  44. extern u8 zuopwm;
  45. extern u8 youpwm;
  46. extern u8 countH1;
  47. extern u8 countH2;
  48. extern u8 countH3;
  49. extern u8 countH4;
  50. extern u8 countH5;
  51. extern u8 countH6;
  52. extern u8 countH7;
  53. extern u8 countH8;
  54. extern u8 countZ1;
  55. extern u8 countY1;
  56. extern u8 zhixing;
  57. extern u16 ServoPwmDuty[5]        ;
  58. extern u16 ServoPwmDutySet[5];
  59. extern u16 times;
  60. unsigned int code_data;
  61. int code_flag;
  62. int yaw1;
  63. u16 ryz;
  64. u16 byz;
  65. u16 gyz;
  66. u16 rr;
  67. u16 bb;
  68. u16 gg;
  69. u16 ryzcount=0;
  70. u16 byzcount=0;
  71. u16 gyzcount=0;       
  72. int main(void)
  73. {       
  74.         u8 flag1;
  75.         float pitch,roll,yaw;                 //歐拉角


  78.         Stm32_Clock_Init(9);        //系統(tǒng)時鐘設(shè)置
  79.         delay_init(72);
  80.         uart_init(72,115200);
  81.         uart2_init(32,9600);
  82.   ServoPwmDuty[0]=a;
  83.         ServoPwmDuty[1]=b;
  84.         ServoPwmDuty[2]=c;
  85.         ServoPwmDuty[3]=d;
  86.         ServoPwmDuty[4]=e;
  87.   ServoPwmDutySet[0]=a;
  88.   ServoPwmDutySet[1]=b;
  89.   ServoPwmDutySet[2]=c;
  90.   ServoPwmDutySet[3]=c;
  91.   ServoPwmDutySet[4]=d;        //延時初始化
  92.   countH1=0;
  93.   countH2=0;
  94.   countH3=0;
  95.   countH4=0;
  96.   countH5=0;
  97.   countH6=0;
  98.   countH7=0;
  99.   countH8=0;
  100.   countZ1=0;
  101.   countY1=0;
  102.         zhixing=0;
  103.   code_flag=0;       
  104.   flag1=1;       
  105.         huidu_init();
  106.         dianji_init();
  107.         exti_init();
  108.         led_init();
  109.        
  110.         tim3_PWM_init(17999,0);//電機(jī)pwm  對應(yīng)pc6,7,8,9，pwm頻率8khz
  111.         tim4_duojiPWM_init(3599,399);
  112.   tim2_duojiPWM_init(3599,399);  //pwm產(chǎn)生
  113.   //tim7_duoji_init(719,3999);  //舵機(jī)速度控制
  114.         //TIM6_init(3599,199);    //用于延時10MS  發(fā)生一次中斷
  115.         tim1_init(700,0);//顏色傳感器計數(shù)
  116.         while(MPU_Init()!=0)printf("over1\r\n");                                        //初始化MPU6050
  117. while(mpu_dmp_init()!=0)printf("over2\r\n");
  118. while(1)
  119.         {
  120.                 while(mpu_dmp_get_data(&pitch,&roll,&yaw));
  121.                 printf("pitch=%f,roll=%f,yaw=%f\n",pitch,roll,yaw);
  122.                
  123.                
  124.         }
  125.         printf("初始化成功");
  126.         while(1);
  127.        
  128. //        delayms(200);
  129. //                while(1)
  130. //                {
  131. //                        i++;
  132. //                       
  133. //                        color_measure();
  134. //                        delayms(200);
  135. //                        ryzcount+=rr;
  136. //                        byzcount+=bb;
  137. //                        gyzcount+=gg;
  138. //                        printf("%d**%d**%d\n",rr,gg,bb);
  139. //                        if(i>=10)
  140. //                        {
  141. //                                printf("%d**%d**%d\n",ryzcount/10,gyzcount/10,byzcount/10);
  142. //                                while(1);
  143. //                        }
  144. //                       
  145. //                }               
  146.                 while(1)
  147.                 {
  148.                         Digital___8();
  149.                 }
  150.                 drive(60,60);
  151.                
  152.         countZ1=0;
  153.         while(1)
  154.         {
  155.                 if(countZ1>=1)
  156.                 {
  157.                         drive(80,-80);
  158.                         break;
  159.                 }
  160.         }
  161.        
  162.         //delayms(630);
  163.         countZ1=0;
  164.         while(1)
  165.         {
  166.           Digital_8();
  167.                 if(countZ1>=6)
  168.                         break;
  169.         }
  170.         drive(90,-90);
  171.         //delayms(1000);
  172.         qianjin();
  173. //        delayms(100);
  174.         countZ1=0;
  175.         while(1)
  176.         {
  177.                 Digital_8();
  178.                
  179.                 if(countZ1>=2)
  180.                         break;
  181.         }
  182.         drive(-30,-30);
  183.         //delayms(300);
  184.         stop();
  185.        
  186.      
  187.        
  188.        
  189.                
  190.                
  191.        
  192.        
  193.        
  194.         while(1)
  195.         {
  196.                 printf("**%d**%d\n",countY1,countZ1);
  197.                 delay_ms(1000);
  198.                 delay_ms(1000);
  199.                 delay_ms(1000);
  200.                 LED0=~LED0;
  201.         }
  202.        
  203.        
  204.        

  206.        
  207.           
  208. }
復(fù)制代碼

所有資料51hei提供下載:
加入6250的嘗試2-dmp - 副本.7z (141.97 KB, 下載次數(shù): 55)


作者: jianyun416    時間: 2019-5-22 22:13
我來驗證一下
作者: 1111..    時間: 2020-4-9 20:45
jianyun416 發(fā)表于 2019-5-22 22:13
我來驗證一下

怎么樣��？朋友。能用嗎



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