求解MPU6050陀螺仪是怎么测角度的

2019-07-15 16:16发布

  1. <p>//****************************************
  2. // MPU6050 IIC测试程序
  3. // 使用单片机STC89C52
  4. // 晶振:11.0592M
  5. // 显示:LCD1602
  6. // 编译环境 Keil uVision2
  7. // 参考宏晶网站24c04通信程序
  8. // 功能: 显示加速度计和陀螺仪的10位原始数据
  9. // 时间:2013年3月1日
  10. //****************************************
  11. #include <REG52.H>
  12. #include <math.h>    //Keil library
  13. #include <stdio.h>   //Keil library
  14. #include <INTRINS.H>
  15. typedef unsigned char  uchar;
  16. typedef unsigned short ushort;
  17. typedef unsigned int   uint;
  18. //****************************************
  19. // 定义51单片机端口
  20. //****************************************
  21. #define DataPort P0                //LCD1602数据端口
  22. sbit    SCL=P1^0;                        //IIC时钟引脚定义
  23. sbit    SDA=P1^1;                        //IIC数据引脚定义
  24. sbit    LCM_RS=P2^6;                //LCD1602命令端口
  25. sbit    LCM_RW=P2^5;                //LCD1602命令端口
  26. sbit    LCM_EN=P2^7;                //LCD1602命令端口

  27. //****************************************
  28. // 定义MPU6050内部地址
  29. //****************************************
  30. #define        SMPLRT_DIV                0x19        //陀螺仪采样率,典型值:0x07(125Hz)
  31. #define        CONFIG                        0x1A        //低通滤波频率,典型值:0x06(5Hz)
  32. #define        GYRO_CONFIG                0x1B        //陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s)
  33. #define        ACCEL_CONFIG        0x1C        //加速计自检、测量范围及高通滤波频率,典型值:0x01(不自检,2G,5Hz)
  34. #define        ACCEL_XOUT_H        0x3B
  35. #define        ACCEL_XOUT_L        0x3C
  36. #define        ACCEL_YOUT_H        0x3D
  37. #define        ACCEL_YOUT_L        0x3E
  38. #define        ACCEL_ZOUT_H        0x3F
  39. #define        ACCEL_ZOUT_L        0x40
  40. #define        TEMP_OUT_H                0x41
  41. #define        TEMP_OUT_L                0x42
  42. #define        GYRO_XOUT_H                0x43
  43. #define        GYRO_XOUT_L                0x44
  44. #define        GYRO_YOUT_H                0x45
  45. #define        GYRO_YOUT_L                0x46
  46. #define        GYRO_ZOUT_H                0x47
  47. #define        GYRO_ZOUT_L                0x48
  48. #define        PWR_MGMT_1                0x6B        //电源管理,典型值:0x00(正常启用)
  49. #define        WHO_AM_I                        0x75        //IIC地址寄存器(默认数值0x68,只读)
  50. #define        SlaveAddress        0xD0        //IIC写入时的地址字节数据,+1为读取
  51. //****************************************
  52. //定义类型及变量
  53. //****************************************
  54. uchar dis[4];                                                        //显示数字(-511至512)的字符数组
  55. int        dis_data;                                                //变量
  56. //int        Temperature,Temp_h,Temp_l;        //温度及高低位数据
  57. //****************************************
  58. //函数声明
  59. //****************************************
  60. void  delay(unsigned int k);                                                                                //延时
  61. //LCD相关函数
  62. void  InitLcd();                                                                                                                //初始化lcd1602
  63. void  lcd_printf(uchar *s,int temp_data);
  64. void  WriteDataLCM(uchar dataW);                                                                        //LCD数据
  65. void  WriteCommandLCM(uchar CMD,uchar Attribc);                                //LCD指令
  66. void  DisplayOneChar(uchar X,uchar Y,uchar DData);                        //显示一个字符
  67. void  DisplayListChar(uchar X,uchar Y,uchar *DData,L);        //显示字符串
  68. //MPU6050操作函数
  69. void  InitMPU6050();                                                                                                        //初始化MPU6050
  70. void  Delay5us();
  71. void  I2C_Start();
  72. void  I2C_Stop();

  73. void  I2C_SendACK(bit ack);
  74. bit   I2C_RecvACK();

  75. void  I2C_SendByte(uchar dat);
  76. uchar I2C_RecvByte();

  77. void  I2C_ReadPage();
  78. void  I2C_WritePage();
  79. void  display_ACCEL_x();
  80. void  display_ACCEL_y();
  81. void  display_ACCEL_z();
  82. uchar Single_ReadI2C(uchar REG_Address);                                                //读取I2C数据
  83. void  Single_WriteI2C(uchar REG_Address,uchar REG_data);        //向I2C写入数据


  84. //****************************************
  85. //整数转字符串
  86. //****************************************
  87. void lcd_printf(uchar *s,  int temp_data)
  88. {
  89.         if(temp_data<0)
  90.         {
  91.                 temp_data=-temp_data;
  92.                 *s='-';
  93.         }
  94.         else *s=' ';
  95.         *++s =temp_data/100+0x30;
  96.         temp_data=temp_data%100;     //取余运算
  97.         *++s =temp_data/10+0x30;
  98.         temp_data=temp_data%10;      //取余运算
  99.         *++s =temp_data+0x30;
  100. }
  101. //****************************************
  102. //延时
  103. //****************************************
  104. void delay(unsigned int k)
  105. {
  106.         unsigned int i,j;
  107.         for(i=0;i<k;i++)
  108.         {
  109.                 for(j=0;j<121;j++);
  110.         }
  111. }
  112. //****************************************
  113. //LCD1602初始化
  114. //****************************************
  115. void InitLcd()
  116. {
  117.         WriteCommandLCM(0x38,1);
  118.         WriteCommandLCM(0x08,1);
  119.         WriteCommandLCM(0x01,1);
  120.         WriteCommandLCM(0x06,1);
  121.         WriteCommandLCM(0x0c,1);
  122.         DisplayOneChar(0,0,'A');
  123.         DisplayOneChar(0,1,'G');
  124. }
  125. //****************************************
  126. //LCD1602写允许
  127. //****************************************
  128. void WaitForEnable(void)
  129. {
  130.         DataPort=0xff;
  131.         LCM_RS=0;LCM_RW=1;_nop_();
  132.         LCM_EN=1;_nop_();_nop_();
  133.         while(DataPort&0x80);
  134.         LCM_EN=0;
  135. }
  136. //****************************************
  137. //LCD1602写入命令
  138. //****************************************
  139. void WriteCommandLCM(uchar CMD,uchar Attribc)
  140. {
  141.         if(Attribc)WaitForEnable();
  142.         LCM_RS=0;LCM_RW=0;_nop_();
  143.         DataPort=CMD;_nop_();
  144.         LCM_EN=1;_nop_();_nop_();LCM_EN=0;
  145. }
  146. //****************************************
  147. //LCD1602写入数据
  148. //****************************************
  149. void WriteDataLCM(uchar dataW)
  150. {
  151.         WaitForEnable();
  152.         LCM_RS=1;LCM_RW=0;_nop_();
  153.         DataPort=dataW;_nop_();
  154.         LCM_EN=1;_nop_();_nop_();LCM_EN=0;
  155. }
  156. //****************************************
  157. //LCD1602写入一个字符
  158. //****************************************
  159. void DisplayOneChar(uchar X,uchar Y,uchar DData)
  160. {
  161.         Y&=1;
  162.         X&=15;
  163.         if(Y)X|=0x40;
  164.         X|=0x80;
  165.         WriteCommandLCM(X,0);
  166.         WriteDataLCM(DData);
  167. }
  168. //****************************************
  169. //LCD1602显示字符串
  170. //****************************************
  171. void DisplayListChar(uchar X,uchar Y,uchar *DData,L)
  172. {
  173.         uchar ListLength=0;
  174.         Y&=0x1;
  175.         X&=0xF;
  176.         while(L--)
  177.         {
  178.                 DisplayOneChar(X,Y,DData[ListLength]);
  179.                 ListLength++;
  180.                 X++;
  181.         }
  182. }

















  183. //**************************************
  184. //延时5微秒(STC90C52RC@12M)
  185. //不同的工作环境,需要调整此函数
  186. //当改用1T的MCU时,请调整此延时函数
  187. //**************************************
  188. void Delay5us()
  189. {
  190.         _nop_();_nop_();_nop_();_nop_();
  191.         _nop_();_nop_();_nop_();_nop_();
  192.         _nop_();_nop_();_nop_();_nop_();
  193.         _nop_();_nop_();_nop_();_nop_();
  194.         _nop_();_nop_();_nop_();_nop_();
  195.         _nop_();_nop_();_nop_();_nop_();
  196. }
  197. //**************************************
  198. //I2C起始信号
  199. //**************************************
  200. void I2C_Start()
  201. {
  202.     SDA = 1;                    //拉高数据线
  203.     SCL = 1;                    //拉高时钟线
  204.     Delay5us();                 //延时
  205.     SDA = 0;                    //产生下降沿
  206.     Delay5us();                 //延时
  207.     SCL = 0;                    //拉低时钟线
  208. }
  209. //**************************************
  210. //I2C停止信号
  211. //**************************************
  212. void I2C_Stop()
  213. {
  214.     SDA = 0;                    //拉低数据线
  215.     SCL = 1;                    //拉高时钟线
  216.     Delay5us();                 //延时
  217.     SDA = 1;                    //产生上升沿
  218.     Delay5us();                 //延时
  219. }
  220. //**************************************
  221. //I2C发送应答信号
  222. //入口参数:ack (0:ACK 1:NAK)
  223. //**************************************
  224. void I2C_SendACK(bit ack)
  225. {
  226.     SDA = ack;                  //写应答信号
  227.     SCL = 1;                    //拉高时钟线
  228.     Delay5us();                 //延时
  229.     SCL = 0;                    //拉低时钟线
  230.     Delay5us();                 //延时
  231. }
  232. //**************************************
  233. //I2C接收应答信号
  234. //**************************************
  235. bit I2C_RecvACK()
  236. {
  237.     SCL = 1;                    //拉高时钟线
  238.     Delay5us();                 //延时
  239.     CY = SDA;                   //读应答信号
  240.     SCL = 0;                    //拉低时钟线
  241.     Delay5us();                 //延时
  242.     return CY;
  243. }
  244. //**************************************
  245. //向I2C总线发送一个字节数据
  246. //**************************************
  247. void I2C_SendByte(uchar dat)
  248. {
  249.     uchar i;
  250.     for (i=0; i<8; i++)         //8位计数器
  251.     {
  252.         dat <<= 1;              //移出数据的最高位
  253.         SDA = CY;               //送数据口
  254.         SCL = 1;                //拉高时钟线
  255.         Delay5us();             //延时
  256.         SCL = 0;                //拉低时钟线
  257.         Delay5us();             //延时
  258.     }
  259.     I2C_RecvACK();
  260. }
  261. //**************************************
  262. //从I2C总线接收一个字节数据
  263. //**************************************
  264. uchar I2C_RecvByte()
  265. {
  266.     uchar i;
  267.     uchar dat = 0;
  268.     SDA = 1;                    //使能内部上拉,准备读取数据,
  269.     for (i=0; i<8; i++)         //8位计数器
  270.     {
  271.         dat <<= 1;
  272.         SCL = 1;                //拉高时钟线
  273.         Delay5us();             //延时
  274.         dat |= SDA;             //读数据
  275.         SCL = 0;                //拉低时钟线
  276.         Delay5us();             //延时
  277.     }
  278.     return dat;
  279. }
  280. //**************************************
  281. //向I2C设备写入一个字节数据
  282. //**************************************
  283. void Single_WriteI2C(uchar REG_Address,uchar REG_data)
  284. {
  285.     I2C_Start();                  //起始信号
  286.     I2C_SendByte(SlaveAddress);   //发送设备地址+写信号
  287.     I2C_SendByte(REG_Address);    //内部寄存器地址,
  288.     I2C_SendByte(REG_data);       //内部寄存器数据,
  289.     I2C_Stop();                   //发送停止信号
  290. }
  291. //**************************************
  292. //从I2C设备读取一个字节数据
  293. //**************************************
  294. uchar Single_ReadI2C(uchar REG_Address)
  295. {
  296.         uchar REG_data;
  297.         I2C_Start();                   //起始信号
  298.         I2C_SendByte(SlaveAddress);    //发送设备地址+写信号
  299.         I2C_SendByte(REG_Address);     //发送存储单元地址,从0开始
  300.         I2C_Start();                   //起始信号
  301.         I2C_SendByte(SlaveAddress+1);  //发送设备地址+读信号
  302.         REG_data=I2C_RecvByte();       //读出寄存器数据
  303.         I2C_SendACK(1);                //接收应答信号
  304.         I2C_Stop();                    //停止信号
  305.         return REG_data;
  306. }










  307. //**************************************
  308. //初始化MPU6050
  309. //**************************************
  310. void InitMPU6050()
  311. {        delay(100);
  312.         Single_WriteI2C(PWR_MGMT_1, 0x00);        //解除休眠状态
  313.         Single_WriteI2C(SMPLRT_DIV, 0x07); //陀螺仪采样率,1KHz
  314.         Single_WriteI2C(CONFIG, 0x06);                 //低通滤波器的设置,截止频率是1K,带宽是5K
  315.         Single_WriteI2C(GYRO_CONFIG, 0x18);
  316.         Single_WriteI2C(ACCEL_CONFIG, 0x01);
  317. }
  318. //**************************************
  319. //在1602上显示10位数据
  320. //**************************************
  321. void Display10BitData  (int value,uchar x,uchar y)
  322. {
  323.         value/=64;                                                //转换为10位数据 value=value/64  右移动六位
  324.         lcd_printf(dis, value);                                //转换数据显示
  325.         DisplayListChar(x, y, dis, 4);                        //启始列,行,显示数组,显示长度
  326. }
  327. //**************************************
  328. //合成数据
  329. //**************************************
  330. int GetData(uchar REG_Address)
  331. {
  332.         char H,L;
  333.         H=Single_ReadI2C(REG_Address);
  334.         L=Single_ReadI2C(REG_Address+1);
  335.         return (H<<8)+L;   //合成数据
  336. }




  337. //**************************************
  338. //显示温度
  339. //**************************************
  340. //void display_temp()
  341. //{
  342. //        Temp_h=Single_ReadI2C(TEMP_OUT_H); //读取温度
  343. //        Temp_l=Single_ReadI2C(TEMP_OUT_L); //读取温度
  344. //        Temperature=Temp_h<<8|Temp_l;     //合成温度
  345. //        Temperature = 35+ ( (double) (Temperature + 13200)) / 280; // 计算出温度
  346. //        lcd_printf(dis,Temperature);     //转换数据显示
  347. //        DisplayListChar(11,1,dis,4);     //启始列,行,显示数组,显示位数
  348. //}
  349. //*********************************************************
  350. //主程序
  351. //*********************************************************
  352. void main()
  353. {
  354.         int  ACCE_X=0,ACCE_Y=0,x=0,y=0,angleAx=0;
  355.         delay(500);                //上电延时
  356.         InitLcd();                //液晶初始化
  357.         InitMPU6050();        //初始化MPU6050
  358.         delay(150);
  359.         while(1)
  360.         {
  361.                 Display10BitData  (GetData(ACCEL_XOUT_H),  2,  0);        //显示X轴加速度
  362.                 Display10BitData  (GetData(ACCEL_YOUT_H),  7,  0);        //显示Y轴加速度
  363.                 Display10BitData  (GetData(ACCEL_ZOUT_H) ,  12,  0);//显示Z轴加速度
  364.                 Display10BitData  (GetData(GYRO_XOUT_H) ,  2,  1);        //显示X轴角速度
  365.                 Display10BitData  (GetData(GYRO_YOUT_H),  7,  1);        //显示Y轴角速度
  366.                 Display10BitData  (GetData(GYRO_ZOUT_H),  12,  1);        //显示Z轴角速度
  367. //                ACCE_X=(GetData(ACCEL_XOUT_H));
  368. //                ACCE_Y=(GetData(ACCEL_YOUT_H));
  369. //                x=(float)ACCE_X/4096.0;
  370. //                y=(float)ACCE_Y/4096.0;
  371. //                angleAx=atan2(x,y)*180/3.14;
  372. //                DisplayOneChar(0,0,'0'+angleAx/100);
  373. //                DisplayOneChar(1,0,'0'+angleAx/10%10);
  374. //                DisplayOneChar(2,0,'0'+angleAx%10);

  375.                
  376.                 delay(500);
  377.         }
  378. }</p>
复制代码
如题。我能得到加速度和角速度。

友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。
该问题目前已经被作者或者管理员关闭, 无法添加新回复
6条回答
ws925301026
1楼-- · 2019-07-15 22:08
谢谢分享!!!谢谢分享!!!谢谢分享!!!谢谢分享!!!谢谢分享!!!谢谢分享!!!
橘子酱
2楼-- · 2019-07-15 23:52
怎么得到角度啊?
cotar
3楼-- · 2019-07-16 02:38
并不能得到角度
mushlee12
4楼-- · 2019-07-16 04:18
謝謝樓主分享!
shao15232
5楼-- · 2019-07-16 06:50
 精彩回答 2  元偷偷看……
飞小飞
6楼-- · 2019-07-16 09:05
博主,如果在一个立体容器里,测试LED灯(在顶端)和传感器(在底部)两者的距离,可以用什么来测?急求

一周热门 更多>