标题:
mpu6050 dmp 欧拉角串口输出 STM32源程序
[打印本页]
作者:
李丽丽lili
时间:
2019-4-27 14:44
标题:
mpu6050 dmp 欧拉角串口输出 STM32源程序
欧拉角输出,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)
2019-4-27 21:46 上传
点击文件名下载附件
下载积分: 黑币 -5
作者:
jianyun416
时间:
2019-5-22 22:13
我来验证一下
作者:
1111..
时间:
2020-4-9 20:45
jianyun416 发表于 2019-5-22 22:13
我来验证一下
怎么样啊?朋友。能用吗
欢迎光临 (http://www.51hei.com/bbs/)
Powered by Discuz! X3.1