mpu6050 dmp 歐拉角串口輸出 STM32源程序

ID:522930 · 發表于 2019-4-27 14:44

歐拉角輸出，pitch roll yaw很精準

單片機源程序如下:

#include "sys.h"
#include "usart.h"
#include "delay.h"
#include "exti.h"
#include "tim.h"
#include "dianji.h"
#include "led.h"
#include "color.h"
#include "exti.h"
#include "duojipwm.h"
#include "huidu.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpuhanshu.h"
#define EXTI_H4_OFF EXTI->IMR&=~(1<<8)
#define EXTI_H6_OFF EXTI->IMR&=~(1<<13)
#define EXTI_Z1_OFF EXTI->IMR&=~(1<<4)
#define EXTI_H4_ON EXTI->IMR|=1<<8
#define EXTI_H6_ON EXTI->IMR|=1<<13
#define EXTI_Z1_ON EXTI->IMR|=1<<4
#define a 95
#define b 170
#define c 60
#define d 240
#define e 0
int dd[22]={70,70,//直行
75,65,//右一微轉
80,60,//右二微轉
90,50,//右三微轉
80,30,//右四微轉
80,20,//右五微轉
65,75,//左一微轉
65,80,//左二微轉
50,90,//左三微轉
30,80,//左四微轉
20,80};//左五微轉
extern u16 zuoma;
extern u16 youma;
extern s16 zuospeed;
extern s16 youspeed;
extern u8 zuopwm;
extern u8 youpwm;
extern u8 countH1;
extern u8 countH2;
extern u8 countH3;
extern u8 countH4;
extern u8 countH5;
extern u8 countH6;
extern u8 countH7;
extern u8 countH8;
extern u8 countZ1;
extern u8 countY1;
extern u8 zhixing;
extern u16 ServoPwmDuty[5] ;
extern u16 ServoPwmDutySet[5];
extern u16 times;
unsigned int code_data;
int code_flag;
int yaw1;
u16 ryz;
u16 byz;
u16 gyz;
u16 rr;
u16 bb;
u16 gg;
u16 ryzcount=0;
u16 byzcount=0;
u16 gyzcount=0;
int main(void)
{
u8 flag1;
float pitch,roll,yaw; //歐拉角
Stm32_Clock_Init(9); //系統時鐘設置
delay_init(72);
uart_init(72,115200);
uart2_init(32,9600);
ServoPwmDuty[0]=a;
ServoPwmDuty[1]=b;
ServoPwmDuty[2]=c;
ServoPwmDuty[3]=d;
ServoPwmDuty[4]=e;
ServoPwmDutySet[0]=a;
ServoPwmDutySet[1]=b;
ServoPwmDutySet[2]=c;
ServoPwmDutySet[3]=c;
ServoPwmDutySet[4]=d; //延時初始化
countH1=0;
countH2=0;
countH3=0;
countH4=0;
countH5=0;
countH6=0;
countH7=0;
countH8=0;
countZ1=0;
countY1=0;
zhixing=0;
code_flag=0;
flag1=1;
huidu_init();
dianji_init();
exti_init();
led_init();
tim3_PWM_init(17999,0);//電機pwm 對應pc6,7,8,9，pwm頻率8khz
tim4_duojiPWM_init(3599,399);
tim2_duojiPWM_init(3599,399); //pwm產生
//tim7_duoji_init(719,3999); //舵機速度控制
//TIM6_init(3599,199); //用于延時10MS 發生一次中斷
tim1_init(700,0);//顏色傳感器計數
while(MPU_Init()!=0)printf("over1\r\n"); //初始化MPU6050
while(mpu_dmp_init()!=0)printf("over2\r\n");
while(1)
{
while(mpu_dmp_get_data(&pitch,&roll,&yaw));
printf("pitch=%f,roll=%f,yaw=%f\n",pitch,roll,yaw);
}
printf("初始化成功");
while(1);
// delayms(200);
// while(1)
// {
// i++;
//
// color_measure();
// delayms(200);
// ryzcount+=rr;
// byzcount+=bb;
// gyzcount+=gg;
// printf("%d**%d**%d\n",rr,gg,bb);
// if(i>=10)
// {
// printf("%d**%d**%d\n",ryzcount/10,gyzcount/10,byzcount/10);
// while(1);
// }
//
// }
while(1)
{
Digital___8();
}
drive(60,60);
countZ1=0;
while(1)
{
if(countZ1>=1)
{
drive(80,-80);
break;
}
}
//delayms(630);
countZ1=0;
while(1)
{
Digital_8();
if(countZ1>=6)
break;
}
drive(90,-90);
//delayms(1000);
qianjin();
// delayms(100);
countZ1=0;
while(1)
{
Digital_8();
if(countZ1>=2)
break;
}
drive(-30,-30);
//delayms(300);
stop();
while(1)
{
printf("**%d**%d\n",countY1,countZ1);
delay_ms(1000);
delay_ms(1000);
delay_ms(1000);
LED0=~LED0;
}
}

復制代碼

所有資料51hei提供下載:

加入6250的嘗試2-dmp - 副本.7z (141.97 KB, 下載次數: 55)

ID:543896 · 發表于 2019-5-22 22:13

我來驗證一下

ID:696834 · 發表于 2020-4-9 20:45

jianyun416 發表于 2019-5-22 22:13
我來驗證一下

怎么樣啊？朋友。能用嗎

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

mpu6050 dmp 歐拉角串口輸出 STM32源程序

評分