12下一页

[经验] 【Landzo C1试用体验】+ 第四篇 ☞头部运动追踪第一步:MPU6050控制舵机

[复制链接]
发表于 2016-8-9 19:56:47   2553 查看 24 回复 显示全部楼层 倒序浏览
分享
        做飞思卡尔智能车比赛的时候看到平衡组的用过这个6050,当时就很感兴趣,一直想玩一下。
        后来我就想这个东西是不是可以用来做头部运动的追踪,没想到经过一天的查资料,在别的论坛这些早都被玩过了。
        经过一天的学习,我实现了我想要的功能,完整的头追要等开学以后才能发帖,我回老家只带了这么点东西回来。
       下面进入正题,
        1.什么是MPU6050?
                6050.png
        MPU6050是一种非常流行的空间运动传感器芯片,可以获取器件当前的三个加速度分量和三个旋转角速度。由于其体积小巧,功能强大,精度较高,不仅被广泛应用于工业,同时也是航模爱好者的神器,被安装在各类飞行器上驰骋蓝天。
  2.所使用套件中的蓝色舵机简介
         舵机.png
        蓝宙的PDF里已经给的很全了,这里不做赘述。
  3.接线示意图
         接线.png
        另外的舵机按照上一节图片中相应去接就可以了。
  4.俯仰角和横滚角
         关于两个角度.png 默认串口显示.png 坐标系.png
        通过阅读上述资料,以及具体的实验观察现象,其对应关系为pitch(俯仰角)和roll(横滚角)具体方法是使用卡尔曼滤波算法中的例程《MPU6050》,这个例程我会上传到附件中。把程序下载到arduino,再打开串口监视器,(这里注意要把波特率修改为115200,和程序中的设定同步,不然你看到的将会是乱码),我们就可以看到第二张图中定义的几个量了。如果需要显示acc和gyro则把程序中的0改为1。
  5.具体实现代码
        通过对第四节和第二节的学习,我们知道舵机的角度是0~180度,而我们通过串口监视器观察到的roll大概是在-90度~90度之间。为了实现同步,在舵机赋值的时候我们把roll加个90再附进去就差不多了。手上硬件全的话,你们就根据实际情况矫正的精准一些,我这里暂时没有这个必要了。我在附件中上传了实验效果的视频,有兴趣的同学可以查看。再添加舵机语句的时候要注意放的位置,不要放到某一个if判断的末尾。添加的语句已经标注为红色。
  1. /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

  2. This software may be distributed and modified under the terms of the GNU
  3. General Public License version 2 (GPL2) as published by the Free Software
  4. Foundation and appearing in the file GPL2.TXT included in the packaging of
  5. this file. Please note that GPL2 Section 2[b] requires that all works based
  6. on this software must also be made publicly available under the terms of
  7. the GPL2 ("Copyleft").

  8. Contact information
  9. -------------------

  10. Kristian Lauszus, TKJ Electronics
  11. Web      :  http://www.tkjelectronics.com
  12. e-mail   :  kristianl@tkjelectronics.com
  13. */
  14. <font color="#ff0000">#include <Servo.h></font>
  15. #include <Wire.h>
  16. #include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter

  17. Servo servo_pin_10;

  18. #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

  19. Kalman kalmanX; // Create the Kalman instances
  20. Kalman kalmanY;

  21. /* IMU Data */
  22. double accX, accY, accZ;
  23. double gyroX, gyroY, gyroZ;
  24. int16_t tempRaw;

  25. double gyroXangle, gyroYangle; // Angle calculate using the gyro only
  26. double compAngleX, compAngleY; // Calculated angle using a complementary filter
  27. double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

  28. uint32_t timer;
  29. uint8_t i2cData[14]; // Buffer for I2C data

  30. // TODO: Make calibration routine

  31. void setup() {
  32.    servo_pin_10.attach(10);
  33.   Serial.begin(115200);
  34.   Wire.begin();
  35. #if ARDUINO >= 157
  36.   Wire.setClock(400000UL); // Set I2C frequency to 400kHz
  37. #else
  38.   TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
  39. #endif

  40.   i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  41.   i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  42.   i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  43.   i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  44.   while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  45.   while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  46.   while (i2cRead(0x75, i2cData, 1));
  47.   if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
  48.     Serial.print(F("Error reading sensor"));
  49.     while (1);
  50.   }

  51.   delay(100); // Wait for sensor to stabilize

  52.   /* Set kalman and gyro starting angle */
  53.   while (i2cRead(0x3B, i2cData, 6));
  54.   accX = (i2cData[0] << 8) | i2cData[1];
  55.   accY = (i2cData[2] << 8) | i2cData[3];
  56.   accZ = (i2cData[4] << 8) | i2cData[5];

  57.   // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  58.   // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  59.   // It is then converted from radians to degrees
  60. #ifdef RESTRICT_PITCH // Eq. 25 and 26
  61.   double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  62.   double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
  63. #else // Eq. 28 and 29
  64.   double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  65.   double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
  66. #endif

  67.   kalmanX.setAngle(roll); // Set starting angle
  68.   kalmanY.setAngle(pitch);
  69.   gyroXangle = roll;
  70.   gyroYangle = pitch;
  71.   compAngleX = roll;
  72.   compAngleY = pitch;

  73.   timer = micros();
  74. }

  75. void loop() {
  76.   /* Update all the values */
  77.   while (i2cRead(0x3B, i2cData, 14));
  78.   accX = ((i2cData[0] << 8) | i2cData[1]);
  79.   accY = ((i2cData[2] << 8) | i2cData[3]);
  80.   accZ = ((i2cData[4] << 8) | i2cData[5]);
  81.   tempRaw = (i2cData[6] << 8) | i2cData[7];
  82.   gyroX = (i2cData[8] << 8) | i2cData[9];
  83.   gyroY = (i2cData[10] << 8) | i2cData[11];
  84.   gyroZ = (i2cData[12] << 8) | i2cData[13];


  85.   double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
  86.   timer = micros();

  87.   // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  88.   // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  89.   // It is then converted from radians to degrees
  90. #ifdef RESTRICT_PITCH // Eq. 25 and 26
  91.   double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  92.   double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
  93. #else // Eq. 28 and 29
  94.   double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  95.   double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
  96. #endif

  97. <font color="#ff0000">    servo_pin_10.write(roll+ 90);</font>

  98.   double gyroXrate = gyroX / 131.0; // Convert to deg/s
  99.   double gyroYrate = gyroY / 131.0; // Convert to deg/s

  100. #ifdef RESTRICT_PITCH
  101.   // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  102.   if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
  103.     kalmanX.setAngle(roll);
  104.     compAngleX = roll;
  105.     kalAngleX = roll;
  106.     gyroXangle = roll;
  107.   } else
  108.     kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  109.   if (abs(kalAngleX) > 90)
  110.     gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
  111.   kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
  112. #else
  113.   // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  114.   if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
  115.     kalmanY.setAngle(pitch);
  116.     compAngleY = pitch;
  117.     kalAngleY = pitch;
  118.     gyroYangle = pitch;
  119.   } else
  120.     kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

  121.   if (abs(kalAngleY) > 90)
  122.     gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
  123.   kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
  124. #endif

  125.   gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  126.   gyroYangle += gyroYrate * dt;
  127.   //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
  128.   //gyroYangle += kalmanY.getRate() * dt;

  129.   compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
  130.   compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

  131.   // Reset the gyro angle when it has drifted too much
  132.   if (gyroXangle < -180 || gyroXangle > 180)
  133.     gyroXangle = kalAngleX;
  134.   if (gyroYangle < -180 || gyroYangle > 180)
  135.     gyroYangle = kalAngleY;
  136.       

  137.   /* Print Data */
  138. #if 0 // Set to 1 to activate
  139.   Serial.print(accX); Serial.print("\t");
  140.   Serial.print(accY); Serial.print("\t");
  141.   Serial.print(accZ); Serial.print("\t");

  142.   Serial.print(gyroX); Serial.print("\t");
  143.   Serial.print(gyroY); Serial.print("\t");
  144.   Serial.print(gyroZ); Serial.print("\t");

  145.   Serial.print("\t");
  146. #endif

  147.   Serial.print(roll); Serial.print("\t");
  148.   Serial.print(gyroXangle); Serial.print("\t");
  149.   Serial.print(compAngleX); Serial.print("\t");
  150.   Serial.print(kalAngleX); Serial.print("\t");

  151.   Serial.print("\t");

  152.   Serial.print(pitch); Serial.print("\t");
  153.   Serial.print(gyroYangle); Serial.print("\t");
  154.   Serial.print(compAngleY); Serial.print("\t");
  155.   Serial.print(kalAngleY); Serial.print("\t");

  156. #if 0 // Set to 1 to print the temperature
  157.   Serial.print("\t");

  158.   double temperature = (double)tempRaw / 340.0 + 36.53;
  159.   Serial.print(temperature); Serial.print("\t");
  160. #endif

  161.   Serial.print("\r\n");
  162.   delay(2);
  163. }
复制代码
  6.应用展望
        如果我手上有两个舵机,那么水平面和竖直面就都可以转动,配合上程序,我们就可以远程的更加人性化的控制摄像头的转动,再把图像显示在头盔中,从而改善现有的监控设备;
        如果我把它装在一辆联网的载具上,就可以远程看到正常人坐在载具中驾驶室的图像,配合上远程操控设备从而时间远程驾驶;
        如果我们在人的鞋子里装上加速度计,通过不同的值来换算成小车的移动速度以及轨迹,再配合上其他传感器,那么,是不是就有可能做出一台跟着人同步走的车?进而我们就可以把书包什么的都丢到车身上,然后我们就可以跑步回家锻炼身体而不是挤公交了。






KalmanFilter-master.zip

18.92 KB, 下载次数: 55, 下载积分: 积分 -1 分

MPU6050卡尔曼滤波

MPU6050简单控制舵机.zip

423.86 KB, 下载次数: 125, 下载积分: 积分 -1 分

标签:MPU6050 舵机
发表于 2016-8-9 20:02:08    楼主|
担心给读者的感觉不够直接,上图,
QQ图片20160809200100.jpg
回复

点赞

技术员

发表于 2016-9-23 08:32:52  

PCB在线计价下单

板子大小:

cm
X
cm

层数:

2

板子数量:

10

厚度:

1.6
学习了,感谢分享!!
回复

技术员

发表于 2016-10-31 14:33:36  
謝謝樓主分享!
回复

助理工程师

发表于 2016-11-1 14:52:45  
。。。。。。。。。。。。。。。。
回复

等待验证会员

发表于 2016-11-2 22:32:07   来自手机
我只想把这个装在hmz t1上把它改造成vr,要怎么没实现呢,需要什么硬件和软件呢
回复

点赞

发表于 2016-11-3 13:20:43    楼主|
好的够了没有 发表于 2016-11-2 22:32
我只想把这个装在hmz t1上把它改造成vr,要怎么没实现呢,需要什么硬件和软件呢

我不知道hmz t1是什么啊
回复

点赞

等待验证会员

发表于 2016-11-8 14:15:33  
卡尔曼滤波不错,是要找的。
回复

点赞

技术员

发表于 2016-11-14 16:24:26  
好玩意
回复

技术员

发表于 2016-11-14 16:35:30  
antimatter2333 发表于 2016-8-9 20:02
担心给读者的感觉不够直接,上图,

程序编译错误
回复

点赞

实习生

发表于 2016-12-1 09:56:35  
在这找到了卡尔曼滤波,十分谢谢!
回复

点赞

发表于 2016-12-19 13:36:05    楼主|
zjy586 发表于 2016-12-1 09:56
在这找到了卡尔曼滤波,十分谢谢!

有用就好,没想到还真能帮到别人呢
回复

点赞

工程师

发表于 2016-12-28 09:35:19  

謝謝樓主分享!
回复

实习生

发表于 2017-2-18 20:55:15  
很好,谢谢
回复

等待验证会员

发表于 2017-3-5 19:03:27  
则个真么下载工
回复

点赞

等待验证会员

发表于 2017-3-5 19:12:14  
编译错误。这什么原因啊

Arduino: 1.5.4 (Windows 7), Board: "Arduino Uno"

sketch_mar05a:27: error: 'Kalman' does not name a type
sketch_mar05a:28: error: 'Kalman' does not name a type
sketch_mar05a.ino: In function 'void setup()':
sketch_mar05a:58: error: 'i2cWrite' was not declared in this scope
sketch_mar05a:59: error: 'i2cWrite' was not declared in this scope
sketch_mar05a:61: error: 'i2cRead' was not declared in this scope
sketch_mar05a:70: error: 'i2cRead' was not declared in this scope
sketch_mar05a:86: error: 'kalmanX' was not declared in this scope
sketch_mar05a:87: error: 'kalmanY' was not declared in this scope
sketch_mar05a.ino: In function 'void loop()':
sketch_mar05a:98: error: 'i2cRead' was not declared in this scope
sketch_mar05a:122: error: expected primary-expression before '<' token
sketch_mar05a:122: error: 'font' was not declared in this scope
sketch_mar05a:122: error: expected `;' before 'color'
sketch_mar05a:122: error: expected primary-expression before '<' token
sketch_mar05a:122: error: expected primary-expression before '/' token
sketch_mar05a:124: error: expected primary-expression before 'double'
sketch_mar05a:124: error: expected `;' before 'double'
sketch_mar05a:130: error: 'kalmanX' was not declared in this scope
sketch_mar05a:135: error: 'kalmanX' was not declared in this scope
sketch_mar05a:135: error: 'gyroXrate' was not declared in this scope
sketch_mar05a:139: error: 'kalmanY' was not declared in this scope
sketch_mar05a:155: error: 'gyroXrate' was not declared in this scope
sketch_mar05a.ino: In function 'void setup()':
sketch_mar05a:204: error: redefinition of 'void setup()'
sketch_mar05a:44: error: 'void setup()' previously defined here
sketch_mar05a.ino: In function 'void loop()':
sketch_mar05a:209: error: redefinition of 'void loop()'
sketch_mar05a:96: error: 'void loop()' previously defined here

  This report would have more information with
  "Show verbose output during compilation"
  enabled in File > Preferences.
回复

点赞

实习生

发表于 2017-4-18 08:50:14  
LZ编译错误啊
回复

点赞

实习生

发表于 2017-4-18 08:52:35  
楼主可否加个  QQ好友呢   64029637
回复

点赞

等待验证会员

发表于 2017-5-3 19:57:55  
编译有错误怎么回事?
回复

点赞

等待验证会员

发表于 2017-5-3 19:58:31  
Arduino:1.8.2 (Windows 10), 开发板:"Arduino/Genuino Uno"

C:\Users\Administrator\Pictures\Camera Roll\sketch_may03a\sketch_may03a.ino: In function 'void setup()':

sketch_may03a:56: error: 'i2cWrite' was not declared in this scope

   while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once

                                          ^

sketch_may03a:57: error: 'i2cWrite' was not declared in this scope

   while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

                                   ^

sketch_may03a:59: error: 'i2cRead' was not declared in this scope

   while (i2cRead(0x75, i2cData, 1));

                                  ^

sketch_may03a:68: error: 'i2cRead' was not declared in this scope

   while (i2cRead(0x3B, i2cData, 6));

                                  ^

C:\Users\Administrator\Pictures\Camera Roll\sketch_may03a\sketch_may03a.ino: In function 'void loop()':

sketch_may03a:96: error: 'i2cRead' was not declared in this scope

   while (i2cRead(0x3B, i2cData, 14));

                                   ^

sketch_may03a:120: error: expected primary-expression before '<' token

<font color="#ff0000">    servo_pin_10.write(roll+ 90);</font>

^

sketch_may03a:120: error: 'font' was not declared in this scope

<font color="#ff0000">    servo_pin_10.write(roll+ 90);</font>

  ^

sketch_may03a:120: error: expected primary-expression before '<' token

<font color="#ff0000">    servo_pin_10.write(roll+ 90);</font>

                                                        ^

sketch_may03a:120: error: expected primary-expression before '/' token

<font color="#ff0000">    servo_pin_10.write(roll+ 90);</font>

                                                         ^

sketch_may03a:122: error: expected primary-expression before 'double'

   double gyroXrate = gyroX / 131.0; // Convert to deg/s

   ^

sketch_may03a:133: error: 'gyroXrate' was not declared in this scope

     kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

                                        ^

sketch_may03a:153: error: 'gyroXrate' was not declared in this scope

   gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter

                 ^

exit status 1
'i2cWrite' was not declared in this scope

在文件 -> 首选项开启
“编译过程中显示详细输出”选项
这份报告会包含更多信息。
回复

点赞

12下一页
高级模式
您需要登录后才可以回帖 登录 | 注册

专家问答 查看更多>>
关闭

站长推荐 上一条 /9 下一条

小黑屋|手机版|Archiver| 电子发烧友 ( 粤ICP备14022951号-2 )     

GMT+8, 2017-7-21 10:38 , Processed in 0.278913 second(s), 16 queries , Memcache On.

微信扫描
快速回复 返回顶部 返回列表
-

推荐专区

技术干货集中营

专家问答

用户帮助┃咨询与建议┃版主议事

我的提问

工程师杂谈

工程师创意

工程师职场

论坛电子赛事

社区活动专版

发烧友活动

-

嵌入式论坛

单片机/MCU论坛

FPGA|CPLD|ASIC论坛

DSP论坛

嵌入式系统论坛

-

电源技术论坛

电源技术论坛

-

硬件设计论坛

PCB设计论坛

电路设计论坛

电子元器件论坛

控制|传感

总线技术|接口技术

-

测试测量论坛

LabVIEW论坛

Matlab论坛

测试测量技术专区

仪器仪表技术专区

-

EDA设计论坛

multisim论坛

PADS技术论坛

Protel|AD|DXP论坛

Allegro论坛

proteus论坛|仿真论坛

EasyEDA-中国人自已的EDA工具

Orcad论坛

-

综合技术与应用

电机控制

智能电网

光电及显示

参考设计中心

汽车电子技术论坛

医疗电子论坛

-

开源硬件

DFRobot专区

树莓派论坛

智能硬件论坛

开发快智能硬件开发平台

Intel物联网开发者专区

Waveshare

乐美客SBC专区

Arduino论坛

BeagleBone论坛

机器人论坛

创客神器NanoPi

小钢炮CANNON

比派科技banana pi专区

-

无线通信论坛

无线通信技术专区

天线|RF射频|微波|雷达技术

-

IC设计论坛

芯片测试与失效分析

Mixed Signal/SOC[数模混合芯片设计]

Analog/RF IC设计

设计与制造封装测试

-

个人版区

阿东Verilog技术专版

直流马达驱动电路设计

LabVIEW英雄联盟

特权同学FPGA专区

-

厂商专区

灵动微电子 MM32

盈鹏飞嵌入式

TI论坛

TI Deyisupport社区

芯灵思嵌入式论坛

Tisan

米尔科技

庆科社区

WIZnet技术专区

Cypress技术论坛

飞凌嵌入式

Qualcomm技术论坛

英创嵌入式

机智云GoKit论坛

-

检测技术与质量

电磁兼容(EMC)设计与整改

安规知识论坛

检测与认证

-

消费电子论坛

手机技术论坛

平板电脑/mid论坛

音视/视频/机顶盒论坛

-

电子论坛综合区

聚丰众筹官方社区

新人报道区

聚丰供应链

-

论坛服务区

-

供求信息发布

供需广告

招聘┃求职发布区

电子展览展会专区