制作平衡车-MPU6050实验

zaqwsx626

今晚用了一些时间试了下6050，参考了一下黑马的程序
我的自平衡小车D2
和飞思卡尔直立车方案

上代码

1. #include "Wire.h"
   
2. #include "I2Cdev.h"
   
3. #include "MPU6050.h"
   
4. 
   
5. #define pi 3.14159
   
6. #define k 40//比例
   
7. 
   
8. MPU6050 accelgyro;
   
9. 
   
10. 
    
11. int16_t ax, ay, az;
    
12. int16_t gx, gy, gz;
    
13. 
    
14. int16_t ax0, ay0, az0;
    
15. int16_t gx0, gy0, gz0;
    
16. 
    
17. unsigned long time;
    
18. float angleG;
    
19. 
    
20. void setup()
    
21. {
    
22. 
    
23.   //接入i2c总线
    
24.   Wire.begin();
    
25.   Serial.begin(115200);
    
26.   //初始化设备
    
27.   Serial.println("Initializing I2C devices...");
    
28.   accelgyro.initialize();
    
29. 
    
30.   //链接设备
    
31.   Serial.println("Testing device connections...");
    
32.   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    
33.   
    
34.   time=millis();
    
35.   offset();//测出陀螺仪偏移量，加计？
    
36.   Serial.println(millis()-time);
    
37. }
    
38. 
    
39. void offset()
    
40. {
    
41.   accelgyro.getMotion6(&ax0, &ay0, &az0, &gx0, &gy0, &gz0);
    
42.   for(int i=0;i<1000;i++)
    
43.   {
    
44.     accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
45.     //ax0=(ax+ax0)/2;
    
46.     //ay0=(ay+ay0)/2;
    
47.     //az0=(az+az0)/2;
    
48. 
    
49.     gx0=(gx+gx0)/2;
    
50.     gy0=(gy+gy0)/2;
    
51.     gz0=(gz+gz0)/2;
    
52.   }
    
53. }
    
54. 
    
55. 
    
56. void loop()
    
57. {  
    
58.   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
59.   int dt=millis();
    
60.   
    
61.   //Serial.print(ax); Serial.print(",");
    
62.   //Serial.print(ay); Serial.print(",");
    
63.   //Serial.print(az); Serial.print(",");
    
64. 
    
65.   //x,y,z轴角速度
    
66.   //Serial.print(gx-gx0); Serial.print(",");
    
67.   //Serial.print(gy-gy0); Serial.print(",");
    
68.   //Serial.println(gz-gz0);
    
69.   float AccX=ax;
    
70.   float AccZ=az;
    
71.   float angleA = atan(AccZ / AccX) * 180 / pi;//加计计算
    
72.   Serial.print(angleA);Serial.print(",");
    
73.   
    
74.   float GyrY=gy - gy0;//减去偏移量
    
75.   dt=millis()-dt;//积分时间
    
76.   float Da=angleA-angleG;//加计算出的角与陀螺积分叫做差
    
77.   angleG = angleG + (k*Da+GyrY/131) * dt / 1000;//陀螺积分
    
78.   Serial.println(angleG);
    
79. }
    

复制代码

angleG = angleG + (k*Da+GyrY/131) * dt / 1000; 差值Da乘比例k后参与积分

依旧有问题：
angleG = angleG + (GyrY/131) * dt / 1000; 这句是原来参考黑马程序改的，可是不知为什么得到的角度与加计得出的角度相差很多，因此想到了飞思卡尔方案中的方法，结果看来还不错。
加速度计该怎么校准，总不能重力去除吧，毕竟用他计算了角度。
SerialChart即时性好像不怎么强，波形跟不上，该怎么解决?

shihaipeng04

嘿嘿，被楼主的图给晃了。好平滑的曲线。仔细看程序，这个滤波是读了很多次，然后用新数值和之前的求平均值？好像最后几次的数值对结果影响比较大？

zaqwsx626

shihaipeng04 发表于 2014-4-1 01:03
嘿嘿，被楼主的图给晃了。好平滑的曲线。仔细看程序，这个滤波是读了很多次，然后用新数值和之前的求平均值 ...

取陀螺仪零飘时确实是多次求平均，至于滤波倒不是求平均。学生一枚，对于滤波这些东西也不太懂，请多指点。

shihaipeng04

zaqwsx626 发表于 2014-4-1 16:51
取陀螺仪零飘时确实是多次求平均，至于滤波倒不是求平均。学生一枚，对于滤波这些东西也不太懂，请多指点 ...

最近也在看各种滤波，我觉得还是学生好，学过的还没忘太多，没学的还有机会。 像我这个早就毙了业的，啥也不会了。诶