把舵机和陀螺仪捆在一起

shihaipeng04

最简陋的自平衡，就是把陀螺仪捆在舵机上，然后用陀螺仪的度数去控制舵机，理论上扭动舵机，陀螺仪就应该保持相对静止的位置上。 这就是自平衡。 但是实际上，会有一些偏差，比如舵机性能差，旋转的时候会有比较明显的抖动，这个抖动传递给陀螺仪，而陀螺仪又会检测到角度变的更大了，于是再次发送更大的角度信号，然后越调节问题越严重。这个问题可以通过更换性能更好的舵机来解决。 另外抓着舵机的手，在陀螺仪眼里和一匹奔跑的马没什么区别，如果你用2只手死死的抓住，会发现舵机会稳当很多。


这是我手工扭动陀螺仪的曲线，明显的毛刺，当然这个问题，还可以通过软件来解决，传说中的各种滤波，让这个参差不齐的曲线变的更平滑。不过目前我还没学会。。

来个代码如果手上有陀螺仪也有舵机的就可以先耍起来了。陀螺仪是adxl345的，舵机无所谓啦，试验正面25克的明显比9克的平稳的多，但是速度不如9的快。
哦对，顺路问一句，为什么adxl345的角度算出来是-270~+270呢？ 为什么不是 255或者90？

1. #include <Wire.h>
   
2. #include <Servo.h>
   
3. 
   
4. //下面的舵机用的变量和类
   
5. Servo myservo;  // create servo object to control a servo
   
6.                 // a maximum of eight servo objects can be created
   
7. 
   
8. int pos = 0;    // variable to store the servo position
   
9. 
   
10. #define Register_ID 0
    
11. #define Register_2D 0x2D
    
12. #define Register_X0 0x32
    
13. #define Register_X1 0x33
    
14. #define Register_Y0 0x34
    
15. #define Register_Y1 0x35
    
16. #define Register_Z0 0x36
    
17. #define Register_Z1 0x37
    
18. 
    
19. //下面是陀螺仪用的变量
    
20. int ADXAddress = 0xA7 >> 1;  // the default 7-bit slave address
    
21. int reading = 0;
    
22. int val=0;
    
23. int X0,X1,X_out;
    
24. double Xg,Yg,Zg;
    
25. 
    
26. 
    
27. //滤波用的变量
    
28. double Filter_Value;
    
29. double Value=0;
    
30. 
    
31. void setup()
    
32. {
    
33.   myservo.attach(9);//舵机的pin口号9
    
34.   Wire.begin();               
    
35.   Serial.begin(9600);   
    
36.   delay(100);
    
37.   // enable to measute g data
    
38.   Wire.beginTransmission(ADXAddress);
    
39.   Wire.write(Register_2D);
    
40.   Wire.write(8);                //measuring enable
    
41.   Wire.endTransmission();     // stop transmitting
    
42. 
    
43. }
    
44. 
    
45. void loop()
    
46. {
    
47.   //--------------X 只取X轴角度
    
48.   Wire.beginTransmission(ADXAddress); // transmit to device
    
49.   Wire.write(Register_X0);
    
50.   Wire.write(Register_X1);
    
51.   Wire.endTransmission();
    
52.   Wire.requestFrom(ADXAddress,2);
    
53.   if(Wire.available()<=2)   
    
54.   {
    
55.     X0 = Wire.read();
    
56.     X1 = Wire.read();
    
57.     X1=X1<<8;
    
58.     X_out=X0+X1;   
    
59.   }
    
60.   Xg=X_out/3.00;  //换算成角度 -90~+90
    
61. 
    
62. 
    
63.   if(((Xg - Value) > 2) || ((Value - Xg) > 2))  //这是个简陋的滤波程序，让数值变化不那么快
    
64.     Filter_Value = Xg;
    
65.   else
    
66.     Filter_Value = Value;
    
67.   Value = Filter_Value;   //小于2度的变化就不调节了，当然你也可以删掉这段滤波代码
    
68. 
    
69.   myservo.write(Filter_Value+90); //调节舵机的位置，+90 是因为舵机是 0~180，而陀螺仪是-90~+90
    
70. //  myservo.write(Xg+90);   //滤波就不起作用了
    
71. 
    
72. //下面是调试显示用的代码，运行的时候连delay一起删掉舵机反应会非常迅速
    
73. Serial.print("X= ");
    
74. Serial.print(Filter_Value);
    
75. Serial.print(",");
    
76. Serial.print(Xg);
    
77. delay(20);
    
78. }
    

复制代码

迷你强

这个貌似挺深的，留个脚印，慢慢看

tangcheng12353

写的貌似不错的样子，看看先

单车Up

adxl345是加速度计{:soso_e127:}

单车Up

单车Up 发表于 2014-5-7 11:04
adxl345是加速度计

加速度计只有在不受外界作用力干扰的情况下（也就是静止或者匀加速运动），才可以用来检测角度