本篇文章给大家谈谈mpu6050c语言程序,以及mpu6050stm32编程对应的知识点,希望对各位有所帮助,不要忘了收藏本站喔。
本文目录一览:
1、谁能给我讲解一下卡尔曼滤波,我最近在用mpu6050,把陀螺仪和加速度的值通过卡尔曼滤波融合。求C程序!2、求51程序,用IIC把MPU6050的原始数据读出来晶振11.0295,单片机型号STC98C523、简单的搬运工C语言代码4、如何同时在一条I2C上同时读两个mpu6050,求程序5、MPu6050怎么用啊,怎么和电脑连起来,上面的针脚有VCC GND SDA SCL XDA XCL AD0INT6、求基于pic16f877a的陀螺仪mpu6050测试,
谁能给我讲解一下卡尔曼滤波,我最近在用mpu6050,把陀螺仪和加速度的值通过卡尔曼滤波融合。求C程序!
给你arduino的卡尔曼滤波融合算法,非原创,我只是封装了算法。
另外你这么难的问题应该给点分才厚道啊!
H文件:
/*
* KalmanFilter.h
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/
#ifndef KalmanFilter_h
#define KalmanFilter_h
#include WProgram.h
class KalmanFilter
{
public:
KalmanFilter();
/*
卡尔曼融合计算
angle_m: 加速度计测量并通过atan2(ax,ay)方法计算得到的角度(弧度值)
gyro_m:陀螺仪测量的角速度值(弧度值)
dt:采样时间(s)
outAngle:卡尔曼融合计算出的角度(弧度值)
outAngleDot:卡尔曼融合计算出的角速度(弧度值)
*/
void getValue(double angle_m, double gyro_m, double dt, double outAngle, double outAngleDot);
private:
double C_0, Q_angle, Q_gyro, R_angle;
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
double angle, angle_dot;
double P[2][2];
double Pdot[4];
};
CPP文件:
/*
* KalmanFilter.cpp
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/
#include “KalmanFilter.h”
KalmanFilter::KalmanFilter()
{
C_0 = 1.0f;
Q_angle = 0.001f;
Q_gyro = 0.003f;
R_angle = 0.5f;
q_bias = angle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f;
angle = angle_dot = 0.0f;
P[0][0] = 1.0f;
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 1.0f;
Pdot[0] = 0.0f;
Pdot[1] = 0.0f;
Pdot[2] = 0.0f;
Pdot[3] = 0.0f;
}
void KalmanFilter::getValue(double angle_m, double gyro_m, double dt, double outAngle, double outAngleDot)
{
/*
Serial.print(“angle_m = “);
Serial.print(angle_m);
Serial.print(“;”);
Serial.print(“gyro_m = “);
Serial.print(gyro_m);
Serial.print(“;”);
*/
angle+=(gyro_m-q_bias) * dt;
angle_err = angle_m – angle;
Pdot[0] = Q_angle – P[0][1] – P[1][0];
Pdot[1] = -P[1][1];
Pdot[2] = -P[1][1];
Pdot[3] = Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;
outAngle = angle;
outAngleDot = angle_dot;
/*
Serial.print(“angle = “);
Serial.print(angle);
Serial.print(“;”);
Serial.print(“angle_dot = “);
Serial.print(angle_dot);
Serial.print(“;”);
*/
}
#endif
求51程序,用IIC把MPU6050的原始数据读出来晶振11.0295,单片机型号STC98C52
用串口显示的
#include REG52.H
#include math.h //Keil library
#include stdio.h //Keil library
#include INTRINS.H
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
//****************************************
// 定义51单片机端口
//****************************************
#define DataPort P0//LCD1602数据端口
sbit SCL=P1^0;//IIC时钟引脚定义
sbit SDA=P1^1;//IIC数据引脚定义
sbit LCM_RS=P2^0;//LCD1602命令端口
sbit LCM_RW=P2^1;//LCD1602命令端口
sbit LCM_EN=P2^2;//LCD1602命令端口
//****************************************
// 定义MPU6050内部地址
//****************************************
#defineSMPLRT_DIV0x19//陀螺仪采样率,典型值:0x07(125Hz)
#defineCONFIG0x1A//低通滤波频率,典型值:0x06(5Hz)
#defineGYRO_CONFIG0x1B//陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s)
#defineACCEL_CONFIG0x1C//加速计自检、测量范围及高通滤波频率,典型值:0x01(不自检,2G,5Hz)
#defineACCEL_XOUT_H0x3B
#defineACCEL_XOUT_L0x3C
#defineACCEL_YOUT_H0x3D
#defineACCEL_YOUT_L0x3E
#defineACCEL_ZOUT_H0x3F
#defineACCEL_ZOUT_L0x40
#defineTEMP_OUT_H0x41
#defineTEMP_OUT_L0x42
#defineGYRO_XOUT_H0x43
#defineGYRO_XOUT_L0x44
#defineGYRO_YOUT_H0x45
#defineGYRO_YOUT_L0x46
#defineGYRO_ZOUT_H0x47
#defineGYRO_ZOUT_L0x48
#definePWR_MGMT_10x6B//电源管理,典型值:0x00(正常启用)
#defineWHO_AM_I0x75//IIC地址寄存器(默认数值0x68,只读)
#defineSlaveAddress0xD0//IIC写入时的地址字节数据,+1为读取
//****************************************
//定义类型及变量
//****************************************
uchar dis[6];//显示数字(-511至512)的字符数组
intdis_data;//变量
//intTemperature,Temp_h,Temp_l;//温度及高低位数据
//****************************************
//函数声明
//****************************************
void delay(unsigned int k);//延时
void lcd_printf(uchar *s,int temp_data);
//MPU6050操作函数
void InitMPU6050();//初始化MPU6050
void Delay5us();
void I2C_Start();
void I2C_Stop();
void I2C_SendACK(bit ack);
bit I2C_RecvACK();
void I2C_SendByte(uchar dat);
uchar I2C_RecvByte();
void I2C_ReadPage();
void I2C_WritePage();
void display_ACCEL_x();
void display_ACCEL_y();
void display_ACCEL_z();
uchar Single_ReadI2C(uchar REG_Address);//读取I2C数据
void Single_WriteI2C(uchar REG_Address,uchar REG_data);//向I2C写入数据
//****************************************
//整数转字符串
//****************************************
void lcd_printf(uchar *s,int temp_data)
{
if(temp_data0)
{
temp_data=-temp_data;
*s=’-‘;
}
else *s=’ ‘;
*++s =temp_data/10000+0x30;
temp_data=temp_data%10000; //取余运算
*++s =temp_data/1000+0x30;
temp_data=temp_data%1000; //取余运算
*++s =temp_data/100+0x30;
temp_data=temp_data%100; //取余运算
*++s =temp_data/10+0x30;
temp_data=temp_data%10; //取余运算
*++s =temp_data+0x30;
}
//****************************************
void SeriPushSend(uchar send_data)
{
SBUF=send_data;
while(!TI);TI=0;
}
//****************************************
//延时
//****************************************
void delay(unsigned int k)
{
unsigned int i,j;
for(i=0;ik;i++)
{
for(j=0;j121;j++);
}
}
//**************************************
//延时5微秒(STC90C52RC@12M)
//不同的工作环境,需要调整此函数
//当改用1T的MCU时,请调整此延时函数
//**************************************
void Delay5us()
{
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
}
//**************************************
//I2C起始信号
//**************************************
void I2C_Start()
{
SDA = 1; //拉高数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 0; //产生下降沿
Delay5us(); //延时
SCL = 0; //拉低时钟线
}
//**************************************
//I2C停止信号
//**************************************
void I2C_Stop()
{
SDA = 0; //拉低数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 1; //产生上升沿
Delay5us(); //延时
}
//**************************************
//I2C发送应答信号
//入口参数:ack (0:ACK 1:NAK)
//**************************************
void I2C_SendACK(bit ack)
{
SDA = ack; //写应答信号
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
//**************************************
//I2C接收应答信号
//**************************************
bit I2C_RecvACK()
{
SCL = 1; //拉高时钟线
Delay5us(); //延时
CY = SDA; //读应答信号
SCL = 0; //拉低时钟线
Delay5us(); //延时
return CY;
}
//**************************************
//向I2C总线发送一个字节数据
//**************************************
void I2C_SendByte(uchar dat)
{
uchar i;
for (i=0; i8; i++) //8位计数器
{
dat = 1; //移出数据的最高位
SDA = CY; //送数据口
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
I2C_RecvACK();
}
//**************************************
//从I2C总线接收一个字节数据
//**************************************
uchar I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA = 1; //使能内部上拉,准备读取数据,
for (i=0; i8; i++) //8位计数器
{
dat = 1;
SCL = 1; //拉高时钟线
Delay5us(); //延时
dat |= SDA; //读数据
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
return dat;
}
//**************************************
//向I2C设备写入一个字节数据
//**************************************
void Single_WriteI2C(uchar REG_Address,uchar REG_data)
{
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //内部寄存器地址,
I2C_SendByte(REG_data); //内部寄存器数据,
I2C_Stop(); //发送停止信号
}
//**************************************
//从I2C设备读取一个字节数据
//**************************************
uchar Single_ReadI2C(uchar REG_Address)
{
uchar REG_data;
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //发送存储单元地址,从0开始
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress+1); //发送设备地址+读信号
REG_data=I2C_RecvByte(); //读出寄存器数据
I2C_SendACK(1); //接收应答信号
I2C_Stop(); //停止信号
return REG_data;
}
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00);//解除休眠状态
Single_WriteI2C(SMPLRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG, 0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x01);
}
//**************************************
//合成数据
//**************************************
int GetData(uchar REG_Address)
{
uchar H,L;
H=Single_ReadI2C(REG_Address);
L=Single_ReadI2C(REG_Address+1);
return (H8)+L; //合成数据
}
//**************************************
//在1602上显示10位数据
//**************************************
void Display10BitData(int value,uchar x,uchar y)
{ uchar i;
//value/=64;//转换为10位数据
lcd_printf(dis, value);//转换数据显示
for(i=0;i6;i++)
{
SeriPushSend(dis[i]);
}
// DisplayListChar(x,y,dis,4);//启始列,行,显示数组,显示长度
}
//**************************************
//显示温度
//**************************************
//void display_temp()
//{
//Temp_h=Single_ReadI2C(TEMP_OUT_H); //读取温度
//Temp_l=Single_ReadI2C(TEMP_OUT_L); //读取温度
//Temperature=Temp_h8|Temp_l; //合成温度
//Temperature = 35+ ((double) (Temperature + 13200)) / 280; // 计算出温度
//lcd_printf(dis,Temperature); //转换数据显示
//DisplayListChar(11,1,dis,4); //启始列,行,显示数组,显示位数
//}
void init_uart()
{
TMOD=0x21;
TH1=0xfd;
TL1=0xfd;
SCON=0x50;
PS=1; //串口中断设为高优先级别
TR0=1; //启动定时器
TR1=1;
ET0=1; //打开定时器0中断
ES=1;
EA=1;
}
//*********************************************************
//主程序
//*********************************************************
void main()
{
delay(500);//上电延时
//InitLcd();//液晶初始化
init_uart();
InitMPU6050();//初始化MPU6050
delay(150);
while(1)
{
Display10BitData(GetData(ACCEL_XOUT_H),2,0);//显示X轴加速度
Display10BitData(GetData(ACCEL_YOUT_H),7,0);//显示Y轴加速度
Display10BitData(GetData(ACCEL_ZOUT_H),12,0);//显示Z轴加速度
Display10BitData(GetData(GYRO_XOUT_H),2,1);//显示X轴角速度
Display10BitData(GetData(GYRO_YOUT_H),7,1);//显示Y轴角速度
Display10BitData(GetData(GYRO_ZOUT_H),12,1);//显示Z轴角速度
SeriPushSend(0x0d);
SeriPushSend(0x0a);//换行,回车
delay(100);
}
}
简单的搬运工C语言代码
程序在tc3.0下编译通过. 回车键选关,我只做了两关, r键重新开始 p键悔步,只能悔五步啊.不过你可以改#define STEPMAX的值 #define MAX 2 /*游戏中总关数*/#define STARTX 180#define STARTY 80#define …
程序在tc3.0下编译通过. 回车键
选关,我只做了两关, r键重新开始
p键悔步,只能悔五步啊.不过你可以
改#define STEPMAX的值
#define MAX 2 /*游戏中总关数*/
#define STARTX 180
#define STARTY 80
#define BKCOLOR BLACK
#define MANCOLOR RED
#define OBJECTCOLOR YELLOW
#define TIMEINT 2
#define STARNUM 300
#define STEPMAX 5
#includesing.h
#includebios.h
#includesio.h
#includedos.h
#includegraphics.h
#includeconio.h
#includetime.h
#includeslib.h
#includesio.h
#define Key_R 0x1372
#define Key_Up 0x4800
#define Key_Enter 0x1c0d
#define Key_Down 0x5000
#define Key_P 0x1970
#define Key_Esc 0x11b
#define Key_Right 0x4d00
#define Key_Left 0x4b00
typedef suct star
{
int x;
int y;
int c;
}Star;
Star s;
typedef suct record
{
char name;
int second;
suct time t;
suct date d;
}Record;
Record r;
typedef suct c
{
int x;
int y;
}Add;
typedef suct a
{
int x;
int y;
}Player;
Player p;
char name; /*进入游戏时记录玩家的姓名*/
time_t t1,t2; /*游戏结束时的时间*/
int Ide; /*开始进入游戏时,选择菜单时返的功能号(1,2,3)*/
int MissionNum; /*玩家正在玩的关数*/
int BoxNum; /*目的地的个数*/
int Key; /*玩家按键*/
int map; /*地图.(空=0),(人=1),(箱子=2),(墙=3),(目的地=4),(人 目的地=5),(箱子 目的地=6)*/
int StepNum;
int DirectionKey; /*用来表示方向键最后一次按键*/
int BoxMove;
int Step;
/*函数定义*/
void InputName();
void Init();
void MainMenu();
void JudgeRecord();
void WriteRecord();
void JudgeIde();
void DrawMenu(int );
void Game();
void InitMission(int );
void NextMission();
void InitPic(int ,int ,int );
int Move(Add );
void DrawWall(int ,int );
void DrawBack(int ,int );
void DrawBox(int ,int );
void DrawObject(int ,int );
void DrawMan(int ,int );
void DrawStar();
int JudgeWin();
void InitMission1();
void InitMission2();
int TimeCome();
void ChangeStar();
void InputName();
void ViewRecords();
void DeleteRecords();
void RegisterStep();
void ReverselyMove();
void MoveBack(Add );
void main()
{
InputName();
Init(); /*驱动显卡*/
srand(time(t1));
MainMenu(); /*画开始菜单(1.start game 2.view records 3.delete records 4.exit game)*/
}
void InputName()
{
char c;
clrscr();
do
{
printf(“\n\nPlease input your name:”);
scanf(“%s”,name);
printf(“Are you sure the name right(Y/N):”);
do
{
c=getch();
}while(c!=’Y’c!=’y’c!=’N’c!=’n’);
}while(c!=’Y’c!=’y’);
}
void Init()
{
int gd=DETECT,gm;
initgraph(gd,gm,”c:\\tc”);
}
void MainMenu()
{
setbkcolor(BKCOLOR);
cleardevice();
/*DrawStar(); /*画开始菜单的背景画面*/
DrawStar();
Ide=0,Key=0;
DrawMenu(Ide);
do
{
if(bioskey(1)) /*有键按下则处理按键*/
{
Key=bioskey(0);
switch(Key)
{
case Key_Down: {Ide ;Ide=Ide%4;DrawMenu(Ide);break;}
case Key_Up: {Ide–;Ide=(Ide 4)%4;DrawMenu(Ide);break;}
}
}
else {if(TimeCome()) ChangeStar();} /*改变背景*/
}while(Key!=Key_Enter);
JudgeIde(); /*根据Ide运行不同的程序*/
}
void JudgeIde()
{
switch(Ide)
{
case 0:Game();break;
case 1:{ViewRecords();bioskey(0);MainMenu();break;}
case 2:{DeleteRecords();ViewRecords();bioskey(0);MainMenu();break;}
case 3:exit(0);
}
}
void Game()
{
int i,j,flag;
Add ad;
MissionNum=0;
NextMission();
do
{
flag=0;
Key=bioskey(0);
switch(Key)
{
case Key_Enter:{NextMission();time(t1);break;}
case Key_Up:{ad.x=-1;ad.y=0;flag=1;DirectionKey=Key;break;}
case Key_Down:{ad.x=1;ad.y=0;flag=1;DirectionKey=Key;break;}
case Key_Left:{ad.x=0;ad.y=-1;flag=1;DirectionKey=Key;break;}
case Key_Right:{ad.x=0;ad.y=1;flag=1;DirectionKey=Key;break;}
case Key_R:{MissionNum–;NextMission();break;}
case Key_Esc:{MainMenu();break;}
case Key_P:{ReverselyMove();break;}
}
if(flag==1)
{if(Move(ad)) {RegisterStep(); if(JudgeWin()) {JudgeRecord();NextMission();}}}
}while(1);
}
void InitMission(int n)
{
int i,j;
for(i=0;i10;i )
for(j=0;j10;j )
map=0;
switch(n)
{
case 1:InitMission1();break; /*第一关*/
case 2:InitMission2();break; /*第二关*/
}
}
void InitPic(int n,int i,int j)
{
switch(n)
{
case 0:DrawBack(i,j);break;
case 1:DrawMan(i,j);break;
case 2:DrawBo
x(i,j);break;
case 3:DrawWall(i,j);break;
case 4:DrawObject(i,j);break;
case 5:DrawMan(i,j);break;
case 6:DrawBox(i,j);break;
}
}
void NextMission()
{
int i,j;
if(MissionNum 1MAX) MissionNum=1;
else MissionNum ;
InitMission(MissionNum);
setbkcolor(BKCOLOR);
cleardevice();
for(i=0;i10;i )
for(j=0;j10;j )
InitPic(map,i,j);
switch(MissionNum)
{
case 1:outtextxy(200,230,”Mission 1″);break;
case 2:outtextxy(200,230,”Mission 2″);break;
}
time(t1);
for(i=0;iSTEPMAX;i )
{Step=BoxMove=0;}
StepNum=0;
}
int Move(Add a)
{
int flag;
int i=StepNum%STEPMAX;
switch(map) /*看下一位置为什么*/
{
case 0:{map-=1;InitPic(map,p.x,p.y);
p.x=p.x a.x;p.y=p.y a.y;
map =1;InitPic(map,p.x,p.y);flag=1;break;}
case 2:{if(map==0||map==4)
{map-=1;map=1;map =2;
InitPic(map,p.x,p.y);
InitPic(map,p.x a.x,p.y a.y);
InitPic(map,p.x 2*a.x,p.y 2*a.y);
p.x=p.x a.x;p.y=p.y a.y;flag=1;BoxMove=1;}
else flag=0;
break;}
case 3:flag=0;break;
case 4:{map-=1;InitPic(map,p.x,p.y);
p.x=p.x a.x;p.y=p.y a.y;
map =1;InitPic(map,p.x,p.y);flag=1;break;}
case 6:{if(map==0||map==4)
{map-=1;map=5;map =2;
InitPic(map,p.x,p.y);
InitPic(map,p.x a.x,p.y a.y);
InitPic(map,p.x 2*a.x,p.y 2*a.y);
p.x=p.x a.x;p.y=p.y a.y;flag=1;BoxMove=1;}
else flag=0;
break;}
}
return flag;
}
void DrawWall(int i,int j)
{
DrawBack(i,j);
setfillstyle(9,1);
bar(STARTX 20*j-9,STARTY 20*i-9,STARTX 20*j 9,STARTY 20*i 9);
}
void DrawMan(int i,int j)
{
DrawBack(i,j);
setcolor(MANCOLOR);
circle(STARTX 20*j,STARTY 20*i,9);
arc(STARTX 20*j-3,STARTY 20*i-2,20,160,3);
arc(STARTX 20*j 4,STARTY 20*i-2,20,160,3);
arc(STARTX 20*j,STARTY 20*i-2,220,320,7);
}
void DrawBack(int i,int j)
{
setfillstyle(1,BKCOLOR);
bar(STARTX 20*j-9,STARTY 20*i-9,STARTX 20*j 9,STARTY 20*i 9);
}
void DrawObject(int i,int j)
{
DrawBack(i,j);
setcolor(OBJECTCOLOR);
line(STARTX 20*j-9,STARTY 20*i,STARTX 20*j 9,STARTY 20*i);
line(STARTX 20*j-9,STARTY 20*i-9,STARTX 20*j 9,STARTY 20*i 9);
line(STARTX 20*j-9,STARTY 20*i 9,STARTX 20*j 9,STARTY 20*i-9);
}
void DrawBox(int i,int j)
{
DrawBack(i,j);
setfillstyle(9,3);
bar(STARTX 20*j-9,STARTY 20*i-9,STARTX 20*j 9,STARTY 20*i 9);
}
void DrawMenu(int j)
{
int n;
char *s={“1.Start Game”,”2.View Records”,”3.Delete Records”,”4.Exit Game”};
settextstyle(0,0,1);
setcolor(GREEN);
for(n=0;n4;n )
outtextxy(250,170 n*20,s);
setcolor(RED);
outtextxy(250,170 j*20,s);
}
void DrawStar()
{
int w,h,i,dotx,doty,color,maxcolor;
w=getmaxx();
h=getmaxy();
maxcolor=getmaxcolor();
for(i=0;iSTARNUM;i )
{
s.x=1 random(w-1);
s.y=1 random(h-1);
s.c=random(maxcolor);
putpixel(s.x,s.y,s.c);
}
}
void ChangeStar()
{
int i,maxcolor;
maxcolor=getmaxcolor();
for(i=0;iSTARNUM;i )
{
s.c=random(maxcolor);
putpixel(s.x,s.y,s.c);
}
}
int TimeCome()
{
static long tm, old;
tm=biostime(0,tm);
if(tm-oldTIMEINT) return 0;
else
{
old=tm; return 1;
}
}
int JudgeWin()
{
int n=0,i,j;
for(i=0;i10;i )
for(j=0;j10;j )
if(map==6) n ;
if(n==BoxNum) return 1;
else return 0;
}
void InitMission1() /*第九关*/
{
int i,j;
for(i=0;i10;i )
for(j=0;j10;j )
map=0;
for(i=0;i=5;i )
map=3;
for(i=5;i=7;i )
{map=map=3;}
for(i=1;i=4;i )
{map=map=map=3;}
map=map=map=3;
for(i=2;i=4;i )
map=2;
map=map=2;
for(i=2;i=3;i )
{map=map=4;}
map=4;
p.x=3;p.y=5;
map=1;
BoxNum=5;
}
void InitMission2()
{
int i,j;
for(i=0;i10;i )
for(j=0;j10;j )
map=0;
for(i=1;i=5;i )
{map=map=3;}
for(i=2;i=4;i )
{map=map=map=map=3;}
map=map=map=map=map=map=3;
map=map=map=map=2;
map=map=map=map=4;
p.x=1;p.y=3;
map=1;
BoxNum=4;
}
void ViewRecords()
{
FILE *fp;
int i;
setbkcolor(BKCOLOR);
cleardevice();
if((fp=fopen(“record”,”r”))==NULL)
{
printf(“\nerror on open file!”);
getch();
exit(1);
}
gotoxy(1,1);
printf(“\n\t\t\tRecord Information\n”);
printf(“Record-holder Achievement(s)\t Time(h:m:s)\t\tDate(y/m/d)”);
for(i=0;iMAX;i )
{fseek(fp,i*sizeof(Record),0);
fread(r,sizeof(Record),1,fp);
printf(“\n%-10s\t%d\t\t d:d:d\t\td/d/d”,r.name,r.second,r.t.ti_hour,r.t.ti_min,r.t.ti_sec,r.d.da_year,r.d.da_mon,r.d.da_day);}
fclose(fp);
gotoxy(10,25);
printf(“Press any key to return mainmenu…”);
}
void DeleteRecords()
{
int i;
FILE *fp;
fp=fopen(“record”,”w”);
for(i=0;iMAX;i )
{
scpy(r.name,”nameless”);
r.second=0;
gettime(r.t);
geate(r.d);
}
for(i=0;iMAX;i )
fwrite(r,sizeof(Record),1,fp);
fclose(fp);
}
void JudgeRecord()
{
int i=MissionNum-1;
time(t2);
if(r.second==0||difftime(t2,t1)r.second)
{
gotoxy(10,3);printf(“\t\tYou have broken the record”);
r.second=difftime(t2,t1);
scpy(r.name,name);
gettime(r.t);
geate(r.d);
WriteRecord();
}
else
{gotoxy(10,3);printf(“\t\tYou have pass this mission”);}
gotoxy(10,4);
printf(“\t\tpress any key continue…”);
getch();
getch();
}
void WriteRecord()
{
FILE *fp;
int i=MissionNum-1;
fp=fopen(“record”,”rt “);
fseek(fp,i*sizeof(Record),0);
fwrite(r,sizeof(Record),1,fp);
fclose(fp);
}
void RegisterStep()
{
int i;
StepNum ;
i=(StepNum-1)%STEPMAX;
Step=DirectionKey;
}
void ReverselyMove()
{
int i;
Add ad;
i=(StepNum-1)%STEPMAX;
if(Step==0) return;
else
{
switch(Step)
{
case Key_Up:{ad.x=1;ad.y=0;MoveBack(ad);break;}
case Key_Down:{ad.x=-1;ad.y=0;MoveBack(ad);break;}
case Key_Left:{ad.x=0;ad.y=1;MoveBack(ad);break;}
case Key_Right:{ad.x=0;ad.y=-1;MoveBack(ad);break;}
}
StepNum–;Step=0;BoxMove=0;
}
}
void MoveBack(Add a) /*一定可以移动*/
{
int i=(StepNum-1)%STEPMAX;
if(BoxMove==0)
{
map-=1;InitPic(map,p.x,p.y);
p.x=p.x a.x;p.y=p.y a.y;
map =1;InitPic(map,p.x,p.y);
}
else if(BoxMove==1)
{
map-=2;InitPic(map,p.x-a.x,p.y-a.y);
map =1;InitPic(map,p.x,p.y);
p.x=p.x a.x;p.y=p.y a.y;
map =1;InitPic(map,p.x,p.y);
}
}
点这里下载
p=map=map=4;
p.x=1;p.y=3;
map=1;
BoxNum=4;
}
void ViewRecords()
{
FILE *fp;
int i;
setbkcolor(BKCOLOR);
cleardevice();
if((fp=fopen(“record”,”r”))==NULL)
{
printf(“\nerror on open file!”);
getch();
exit(1);
}
gotoxy(1,1);
printf(“\n\t\t\tRecord Information\n”);
printf(“Record-holder Achievement(s)\t Time(h:m:s)\t\tDate(y/m/d)”);
for(i=0;iMAX;i )
{fseek(fp,i*sizeof(Record),0);
fread(r,sizeof(Record),1,fp);
printf(“\n%-10s\t%d\t\t d:d:d\t\td/d/d”,r.name,r.second,r.t.ti_hour,r.t.ti_min,r.t.ti_sec,r.d.da_year,r.d.da_mon,r.d.da_day);}
fclose(fp);
gotoxy(10,25);
printf(“Press any key to return mainmenu…”);
}
void DeleteRecords()
{
int i;
FILE *fp;
fp=fopen(“record”,”w”);
for(i=0;iMAX;i )
{
scpy(r.name,”nameless”);
r.second=0;
gettime(r.t);
geate(r.d);
}
for(i=0;iMAX;i )
fwrite(r,sizeof(Record),1,fp);
fclose(fp);
}
void JudgeRecord()
{
int i=MissionNum-1;
time(t2);
if(r.second==0||difftime(t2,t1)r.second)
{
gotoxy(10,3);printf(“\t\tYou have broken the record”);
r.second=difftime(t2,t1);
scpy(r.name,name);
gettime(r.t);
geate(r.d);
WriteRecord();
}
else
{gotoxy(10,3);printf(“\t\tYou have pass this mission”);}
gotoxy(10,4);
printf(“\t\tpress any key continue…”);
getch();
getch();
}
void WriteRecord()
{
FILE *fp;
int i=MissionNum-1;
fp=fopen(“record”,”rt “);
fseek(fp,i*sizeof(Record),0);
fwrite(r,sizeof(Record),1,fp);
fclose(fp);
}
void RegisterStep()
{
int i;
StepNum ;
i=(StepNum-1)%STEPMAX;
Step=DirectionKey;
}
void ReverselyMove()
{
int i;
Add ad;
i=(StepNum-1)%STEPMAX;
if(Step==0) return;
else
{
switch(Step)
{
case Key_Up:{ad.x=1;ad.y=0;MoveBack(ad);break;}
case Key_Down:{ad.x=-1;ad.y=0;MoveBack(ad);break;}
case Key_Left:{ad.x=0;ad.y=1;MoveBack(ad);break;}
case Key_Right:{ad.x=0;ad.y=-1;MoveBack(ad);break;}
}
StepNum–;Step=0;BoxMove=0;
}
}
void MoveBack(Add a) /*一定可以移动*/
{
int i=(StepNum-1)%STEPMAX;
if(BoxMove==0)
{
map-=1;InitPic(map,p.x,p.y);
p.x=p.x a.x;p.y=p.y a.y;
map =1;InitPic(map,p.x,p.y);
}
else if(BoxMove==1)
{
map-=2;InitPic(map,p.x-a.x,p.y-a.y);
map =1;InitPic(map,p.x,p.y);
p.x=p.x a.x;p.y=p.y a.y;
map =1;InitPic(map,p.x,p.y);
}
}
如何同时在一条I2C上同时读两个mpu6050,求程序
你的同时读是什么意思?单核MCU不可能同时读的,总有先后顺序。
如果你的本意是一条I2C总线上挂两个MPU6050,那是可以的。这个芯片的第9脚AD0是设备地址的最低位,两个芯片一个拉高,一个拉低,就有两个不同的设备地址了。
这是个传感芯片,读到的数据需要根据不同应用处理,难道你只想要读数据的函数?自己写呗,很简单的。
MPu6050怎么用啊,怎么和电脑连起来,上面的针脚有VCC GND SDA SCL XDA XCL AD0INT
可以尝试用51单片机的人机交互功能,先把MPU6050先和51单片机连接在一起,然后利用超级终端在电脑上显示数据。
求基于pic16f877a的陀螺仪mpu6050测试,
PIC还没用过 51 和stm8s 的程序就可以用 其实都差不多
读MPU6050数据的文件
#ifndef __MPU6050_H__
#define __MPU6050_H__
sbit SCL=P2^5;//IIC时钟引脚定义
sbit SDA=P2^6;//IIC数据引脚定义
void InitMPU6050();//初始化MPU6050
void Delay5us();
void I2C_Start();
void I2C_Stop();
void I2C_SendACK(bit ack);
bit I2C_RecvACK();
void I2C_SendByte(unsigned char dat);
unsigned char I2C_RecvByte();
void I2C_ReadPage();
void I2C_WritePage();
unsigned char Single_ReadI2C(unsigned char REG_Address);//读取I2C数据
void Single_WriteI2C(unsigned char REG_Address,unsigned char REG_data);//向I2C写入数据
// 定义MPU6050内部地址
#defineSMPLRT_DIV0x19//陀螺仪采样率,典型值:0x07(125Hz)
#defineCONFIG0x1A//低通滤波频率,典型值:0x06(5Hz)
#defineGYRO_CONFIG0x1B//陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s)
#defineACCEL_CONFIG0x1C//加速计自检、测量范围及高通滤波频率,典型值:0x01(不自检,2G,5Hz)
#defineACCEL_XOUT_H0x3B
#defineACCEL_XOUT_L0x3C
#defineACCEL_YOUT_H0x3D
#defineACCEL_YOUT_L0x3E
#defineACCEL_ZOUT_H0x3F
#defineACCEL_ZOUT_L0x40
#defineTEMP_OUT_H0x41
#defineTEMP_OUT_L0x42
#defineGYRO_XOUT_H0x43
#defineGYRO_XOUT_L0x44
#defineGYRO_YOUT_H0x45
#defineGYRO_YOUT_L0x46
#defineGYRO_ZOUT_H0x47
#defineGYRO_ZOUT_L0x48
#definePWR_MGMT_10x6B//电源管理,典型值:0x00(正常启用)
#defineWHO_AM_I0x75//IIC地址寄存器(默认数值0x68,只读)
#defineSlaveAddress0xD0//IIC写入时的地址字节数据,+1为读取
void Delay5us() //误差 -0.083333333333us
{
unsigned char a;
for(a=27;a0;a–);
}
void I2C_Start()
{
SDA = 1; //拉高数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 0; //产生下降沿
Delay5us(); //延时
SCL = 0; //拉低时钟线
}
//**************************************
//I2C停止信号
//**************************************
void I2C_Stop()
{
SDA = 0; //拉低数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 1; //产生上升沿
Delay5us(); //延时
}
//**************************************
//I2C发送应答信号
//入口参数:ack (0:ACK 1:NAK)
//**************************************
void I2C_SendACK(bit ack)
{
SDA = ack; //写应答信号
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
//**************************************
//I2C接收应答信号
//**************************************
bit I2C_RecvACK()
{
SCL = 1; //拉高时钟线
Delay5us(); //延时
CY = SDA; //读应答信号
SCL = 0; //拉低时钟线
Delay5us(); //延时
return CY;
}
//**************************************
//向I2C总线发送一个字节数据
//**************************************
void I2C_SendByte(unsigned char dat)
{
unsigned char i;
for (i=0; i8; i++) //8位计数器
{
dat = 1; //移出数据的最高位
SDA = CY; //送数据口
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
I2C_RecvACK();
}
//**************************************
//从I2C总线接收一个字节数据
//**************************************
unsigned char I2C_RecvByte()
{
unsigned char i;
unsigned char dat = 0;
SDA = 1; //使能内部上拉,准备读取数据,
for (i=0; i8; i++) //8位计数器
{
dat = 1;
SCL = 1; //拉高时钟线
Delay5us(); //延时
dat |= SDA; //读数据
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
return dat;
}
//**************************************
//向I2C设备写入一个字节数据
//**************************************
void Single_WriteI2C(unsigned char REG_Address,unsigned char REG_data)
{
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //内部寄存器地址,
I2C_SendByte(REG_data); //内部寄存器数据,
I2C_Stop(); //发送停止信号
}
//**************************************
//从I2C设备读取一个字节数据
//**************************************
unsigned char Single_ReadI2C(unsigned char REG_Address)
{
unsigned char REG_data;
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //发送存储单元地址,从0开始
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress+1); //发送设备地址+读信号
REG_data=I2C_RecvByte(); //读出寄存器数据
I2C_SendACK(1); //接收应答信号
I2C_Stop(); //停止信号
return REG_data;
}
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00);//解除休眠状态
Single_WriteI2C(SMPLRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG, 0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x01);
}
//**************************************
//合成数据
//**************************************
int GetData(unsigned char REG_Address)
{
char H,L;
H=Single_ReadI2C(REG_Address);
L=Single_ReadI2C(REG_Address+1);
return (H8)+L; //合成数据
}
#endif
下面是滤波
#ifndef __Kalman_H__
#define __Kalman_H__
void Kalman_Filter(float Accel,float Gyro);
void Angle_Calcu(void);
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
//******角度参数************
float Gyro_y; //Y轴陀螺仪数据暂存
float Angle_gy; //由角速度计算的倾斜角度
float Accel_x; //X轴加速度值暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle; //小车最终倾斜角度
uchar value;
//******卡尔曼参数************
float Q_angle=0.001;
float Q_gyro=0.003;
float R_angle=0.5;
float dt=0.01; //dt为kalman滤波器采样时间;
char C_0 = 1;
float Q_bias, Angle_err;
float xdata PCt_0, PCt_1, E;
float xdata K_0, K_1, t_0, t_1;
float xdata Pdot[4] ={0,0,0,0};
float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } };
// 卡尔曼滤波
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro – Q_bias) * dt; //先验估计
Pdot[0]=Q_angle – PP[0][1] – PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel – Angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
Gyro_y = Gyro – Q_bias; //输出值(后验估计)的微分=角速度
}
//*********************************************************
// 倾角计算卡尔曼融合
//在程序中利用Angle+=(Gyro – Q_bias) * dt计算出陀螺仪积分出的角度,其中Q_bias是陀螺仪偏差。
//此时利用陀螺仪积分求出的Angle相当于系统的估计值,得到系统的观测方程;而加速度计检测的角度Accel相当于系统中的测量值,得到系统状态方程。
//程序中Q_angle和Q_gyro分别表示系统对加速度计及陀螺仪的信任度。根据Pdot = A*P + P*A’ + Q_angle计算出先验估计协方差的微分,用于将当前估计值进行线性化处理。其中A为雅克比矩阵。
//随后计算系统预测角度的协方差矩阵P。计算估计值Accel与预测值Angle间的误差Angle_err。
//计算卡尔曼增益K_0,K_1,K_0用于最优估计值,K_1用于计算最优估计值的偏差并更新协方差矩阵P。
//通过卡尔曼增益计算出最优估计值Angle及预测值偏差Q_bias,此时得到最优角度值Angle及角速度值。
void Angle_Calcu(void)
{
//——加速度————————–
//范围为2g时换算关系16384 LSB/g
//角度较小时x=sinx得到角度弧度, deg = rad*180/3.14
//因为x=sinx,故乘以1.3适当放大
Accel_x = GetData(ACCEL_XOUT_H); //读取X轴加速度
Angle_ax = (Accel_x – 1100) /16384; //去除零点偏移,计算得到角度弧度
Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度,
//——-角速度————————-
//范围为2000deg/s时换算关系16.4 LSB/(deg/s)
Gyro_y = GetData(GYRO_YOUT_H); //静止时角速度Y轴输出为-30左右
Gyro_y = -(Gyro_y + 30)/16.4; //去除零点偏移计算角速度值,负号为方向处理
//Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度.
//——-卡尔曼滤波融合———————–
Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
/*//——-互补滤波———————–
//补偿原理是取当前倾角和加速度获得倾角差值进行放大然后与
//陀螺仪角速度叠加后再积分从而使倾角最跟踪为加速度获得的角度
//0.5为放大倍数可调节补偿度0.01为系统周期10ms
Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
}
#endif
把Angle显示出来就行了
mpu6050c语言程序的介绍就聊到这里吧,感谢你花时间阅读本站内容,更多关于mpu6050stm32编程、mpu6050c语言程序的信息别忘了在本站进行查找喔。