arduino学习笔记 - ITG3200-ADXL345做姿态识别实验

sydz

转载自互联网！！！仅供学习
姿态识别应用范围很广，像自平衡车呀，飞行器呀，双足机器人呀之类。本次我们使用Arduino+ITG3205+ADXL345做姿态检测，使用Processing作为输出，实时显示姿态。

本次实验使用的ITG3205与ADXL345都是成品模块，都可以使用I2C接口进行连接。

先看硬件连接，模拟5号口连接I2C模块的SCL，模拟4号口连接I2C模块的SDA口。VCC与GND正常连接，主要不要接错电压，要使用3.3V。I2C模块之间并联。



把下面的代码编译后下载进入arduino控制板中。

1. #include <Wire.h>  // 调用I2C库
   
2. 
   
3. // 加速度传感器 ADXL345
   
4. #define ACC (0x53)    //定义ADXL345地址
   
5. #define A_TO_READ (6)        //读取每次占用的字节数 (每个坐标轴占两个字节)
   
6. 
   
7. 
   
8. // 陀螺仪 ITG3200
   
9. #define GYRO 0x68 // 定义传感器地址,将AD0连接到GND口，传感器地址为二进制数11101000 (请参考你接口板的原理图)
   
10. #define G_SMPLRT_DIV 0x15
    
11. #define G_DLPF_FS 0x16
    
12. #define G_INT_CFG 0x17
    
13. #define G_PWR_MGM 0x3E
    
14. 
    
15. #define G_TO_READ 8 // x,y,z 每个轴2 bytes
    
16. 
    
17. // 陀螺仪误差修正的偏移量
    
18. int g_offx = 67;
    
19. int g_offy = 5;
    
20. int g_offz = 41;
    
21. 
    
22. // 加速度传感器误差修正的偏移量
    
23. int a_offx = -30;
    
24. int a_offy = -8;
    
25. int a_offz = 0;
    
26. 
    
27. char str[512];
    
28. 
    
29. void initAcc() {
    
30.   //调用 ADXL345
    
31.   writeTo(ACC, 0x2D, 0);      
    
32.   writeTo(ACC, 0x2D, 16);
    
33.   writeTo(ACC, 0x2D, 8);
    
34.   //设定在 +-2g 时的默认读数
    
35. }
    
36. 
    
37. void getAccelerometerData(int * result) {
    
38.   int regAddress = 0x32;    //加速度传感器ADXL345第一轴的数据的设定
    
39.   byte buff[A_TO_READ];
    
40. 
    
41.   readFrom(ACC, regAddress, A_TO_READ, buff); //读取加速度传感器ADXL345的数据
    
42. 
    
43.   //每个轴的读数有10位分辨率，即2个字节.  
    
44.   //我们要转换两个bytes为一个int变量
    
45.   result[0] = (((int)buff[1]) << 8) | buff[0] + a_offx;   
    
46.   result[1] = (((int)buff[3]) << 8) | buff[2] + a_offy;
    
47.   result[2] = (((int)buff[5]) << 8) | buff[4] + a_offz;
    
48. }
    
49. 
    
50. //初始化陀螺仪
    
51. void initGyro()
    
52. {
    
53.   /*****************************************
    
54.    * ITG 3200
    
55.    * 电源管理设定：
    
56.    * 时钟选择 =内部振荡器
    
57.    * 无复位, 无睡眠模式
    
58.    * 无待机模式
    
59.    * 采样率 = 125Hz
    
60.    * 参数为+ / - 2000度/秒
    
61.    * 低通滤波器=5HZ
    
62.    * 没有中断
    
63.    ******************************************/
    
64.   writeTo(GYRO, G_PWR_MGM, 0x00);
    
65.   writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF
    
66.   writeTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19
    
67.   writeTo(GYRO, G_INT_CFG, 0x00);
    
68. }
    
69. 
    
70. 
    
71. void getGyroscopeData(int * result)
    
72. {
    
73.   /**************************************
    
74.    * 陀螺仪ITG- 3200的I2C
    
75.    * 寄存器：
    
76.    * temp MSB = 1B, temp LSB = 1C
    
77.    * x axis MSB = 1D, x axis LSB = 1E
    
78.    * y axis MSB = 1F, y axis LSB = 20
    
79.    * z axis MSB = 21, z axis LSB = 22
    
80.    *************************************/
    
81. 
    
82.   int regAddress = 0x1B;
    
83.   int temp, x, y, z;
    
84.   byte buff[G_TO_READ];
    
85. 
    
86.   readFrom(GYRO, regAddress, G_TO_READ, buff); //读取陀螺仪ITG3200的数据
    
87. 
    
88.   result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
    
89.   result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
    
90.   result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
    
91.   result[3] = (buff[0] << 8) | buff[1]; // 温度
    
92. 
    
93. }
    
94. 
    
95. 
    
96. void setup()
    
97. {
    
98.   Serial.begin(9600);
    
99.   Wire.begin();
    
100.   initAcc();
     
101.   initGyro();
     
102. }
     
103. 
     
104. 
     
105. void loop()
     
106. {
     
107.   int acc[3];
     
108.   int gyro[4];
     
109.   getAccelerometerData(acc);
     
110.   getGyroscopeData(gyro);
     
111. 
     
112.   sprintf(str, "%d,%d,%d,%d,%d,%d,%d", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], gyro[3]);  
     
113.   Serial.print(str);
     
114.   Serial.print(10, BYTE);
     
115. 
     
116.   //延时50毫秒
     
117. }
     
118. 
     
119. 
     
120. //---------------- 功能
     
121. //将val写入到加速度传感器的地址寄存器中
     
122. void writeTo(int DEVICE, byte address, byte val) {
     
123.   Wire.beginTransmission(DEVICE); //传送到加速度传感器
     
124.   Wire.send(address);        // 发送寄存器地址
     
125.   Wire.send(val);        // 发送要写入的值
     
126.   Wire.endTransmission(); //结束传输
     
127. }
     
128. 
     
129. 
     
130. //加速度传感器在地址寄存器的缓冲区阵列中读取读数
     
131. void readFrom(int DEVICE, byte address, int num, byte buff[]) {
     
132.   Wire.beginTransmission(DEVICE); //开始传送至加速度传感器
     
133.   Wire.send(address);        //发送读取的地址
     
134.   Wire.endTransmission(); //结束传输
     
135. 
     
136.   Wire.beginTransmission(DEVICE); //开始传送到ACC
     
137.   Wire.requestFrom(DEVICE, num);    // 要求从加速度传感器中发送6个字节的数据
     
138. 
     
139.   int i = 0;
     
140.   while(Wire.available())    //当加速度传感器返回的数据小于要求值时（异常情况）
     
141.   {
     
142.     buff[i] = Wire.receive(); // 接收数据
     
143.     i++;
     
144.   }
     
145.   Wire.endTransmission(); //结束传输
     
146. }
     

复制代码

先介绍一下processing的基本使用方法，先从http://processing.org/download/下载回来processing的IDE。

然后把下面代码拷贝进入进入processing，查看连接arduino的com口是第几个。根据具体情况调整com口连接代码。

1. 
   
2. import processing.serial.*;
   
3. 
   
4. Serial myPort;  // 创建串口对象myPort
   
5. 
   
6. boolean firstSample = true;
   
7. 
   
8. float [] RwAcc = new float[3];         // 通过加速度传感器把重力加速度投影在x/y/z三轴上
   
9. float [] Gyro = new float[3];          // 陀螺仪读取
   
10. float [] RwGyro = new float[3];        // 重新读取陀螺仪
    
11. float [] Awz = new float[2];           // XZ/ YZ平面和Z轴（度）R的投影之间的角度
    
12. float [] RwEst = new float[3];
    
13. 
    
14. 
    
15. int lastTime = 0;
    
16. int interval = 0;
    
17. float wGyro = 10.0;
    
18. 
    
19. int lf = 10; // 10在ASCII表中表示'\n'
    
20. byte[] inBuffer = new byte[100];
    
21. 
    
22. PFont font;
    
23. final int VIEW_SIZE_X = 600, VIEW_SIZE_Y = 600;
    
24. 
    
25. 
    
26. void setup()
    
27. {
    
28.   size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
    
29.   myPort = new Serial(this, Serial.list()[2], 9600); // 设置电脑第三个COM口为连接端口，这个要根据你电脑情况进行设置。
    
30. 
    
31.   //myPort = new Serial(this, "/dev/ttyUSB0", 9600);  
    
32. 
    
33.   // 加载字体，字体必须在代码文件同目录下的data文件夹中
    
34.   font = loadFont("CourierNew36.vlw");
    
35. }
    
36. 
    
37. 
    
38. void readSensors() {
    
39.   if (myPort.available() > 0) {
    
40.     if (myPort.readBytesUntil(lf, inBuffer) > 0) {
    
41.       String inputString = new String(inBuffer);
    
42.       String [] inputStringArr = split(inputString, ',');
    
43. 
    
44.       // 把原始数据转换为G
    
45.       RwAcc[0] = float(inputStringArr[0]) / 256.0;
    
46.       RwAcc[1] = float(inputStringArr[1])/ 256.0;
    
47.       RwAcc[2] = float(inputStringArr[2])/ 256.0;
    
48. 
    
49.       // 把原始数据转换为"度/秒"
    
50.       Gyro[0] = float(inputStringArr[3]) / 14.375;
    
51.       Gyro[1] = float(inputStringArr[4]) / 14.375;
    
52.       Gyro[2] = float(inputStringArr[5]) / 14.375;
    
53.     }
    
54.   }
    
55. }
    
56. 
    
57. 
    
58. void normalize3DVec(float [] vector) {
    
59.   float R;
    
60.   R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
    
61.   vector[0] /= R;
    
62.   vector[1] /= R;  
    
63.   vector[2] /= R;
    
64. }
    
65. 
    
66. 
    
67. float squared(float x) {
    
68.   return x*x;
    
69. }
    
70. 
    
71. 
    
72. void buildBoxShape() {
    
73.   //box(60, 10, 40);
    
74.   noStroke();
    
75.   beginShape(QUADS);
    
76. 
    
77.   //Z+ (绘图区域)
    
78.   fill(#00ff00);
    
79.   vertex(-30, -5, 20);
    
80.   vertex(30, -5, 20);
    
81.   vertex(30, 5, 20);
    
82.   vertex(-30, 5, 20);
    
83. 
    
84.   //Z-
    
85.   fill(#0000ff);
    
86.   vertex(-30, -5, -20);
    
87.   vertex(30, -5, -20);
    
88.   vertex(30, 5, -20);
    
89.   vertex(-30, 5, -20);
    
90. 
    
91.   //X-
    
92.   fill(#ff0000);
    
93.   vertex(-30, -5, -20);
    
94.   vertex(-30, -5, 20);
    
95.   vertex(-30, 5, 20);
    
96.   vertex(-30, 5, -20);
    
97. 
    
98.   //X+
    
99.   fill(#ffff00);
    
100.   vertex(30, -5, -20);
     
101.   vertex(30, -5, 20);
     
102.   vertex(30, 5, 20);
     
103.   vertex(30, 5, -20);
     
104. 
     
105.   //Y-
     
106.   fill(#ff00ff);
     
107.   vertex(-30, -5, -20);
     
108.   vertex(30, -5, -20);
     
109.   vertex(30, -5, 20);
     
110.   vertex(-30, -5, 20);
     
111. 
     
112.   //Y+
     
113.   fill(#00ffff);
     
114.   vertex(-30, 5, -20);
     
115.   vertex(30, 5, -20);
     
116.   vertex(30, 5, 20);
     
117.   vertex(-30, 5, 20);
     
118. 
     
119.   endShape();
     
120. }
     
121. 
     
122. 
     
123. void drawCube() {  
     
124.   pushMatrix();
     
125.   translate(300, 450, 0);
     
126.   scale(4, 4, 4);
     
127. 
     
128.   rotateX(HALF_PI * -RwEst[0]);
     
129.   rotateZ(HALF_PI * RwEst[1]);
     
130. 
     
131.   buildBoxShape();
     
132. 
     
133.   popMatrix();
     
134. }
     
135. 
     
136. 
     
137. void getInclination() {
     
138.   int w = 0;
     
139.   float tmpf = 0.0;
     
140.   int currentTime, signRzGyro;
     
141. 
     
142. 
     
143.   readSensors();
     
144.   normalize3DVec(RwAcc);
     
145. 
     
146.   currentTime = millis();
     
147.   interval = currentTime - lastTime;
     
148.   lastTime = currentTime;
     
149. 
     
150.   if (firstSample || Float.isNaN(RwEst[0])) { // NaN用来等待检查从arduino过来的数据
     
151.     for (w=0;w<=2;w++) {
     
152.       RwEst[w] = RwAcc[w];    // 初始化加速度传感器读数
     
153.     }
     
154.   }
     
155.   else {
     
156.     // 对RwGyro进行评估
     
157.     if (abs(RwEst[2]) < 0.1) {
     
158.       // Rz值非常的小，它的作用是作为Axz与Ayz的计算参照值，防止放大的波动产生错误的结果。
     
159.       // 这种情况下就跳过当前的陀螺仪数据，使用以前的。
     
160.       for (w=0;w<=2;w++) {
     
161.         RwGyro[w] = RwEst[w];
     
162.       }
     
163.     }
     
164.     else {
     
165.       // ZX/ZY平面和Z轴R的投影之间的角度，基于最近一次的RwEst值
     
166.       for (w=0;w<=1;w++) {
     
167.         tmpf = Gyro[w];                        // 获取当前陀螺仪的deg/s
     
168.         tmpf *= interval / 1000.0f;                     // 得到角度变化值
     
169.         Awz[w] = atan2(RwEst[w], RwEst[2]) * 180 / PI;   // 得到角度并转换为度
     
170.         Awz[w] += tmpf;             // 根据陀螺仪的运动得到更新后的角度
     
171.       }
     
172. 
     
173.       // 判断RzGyro是多少，主要看Axz的弧度是多少
     
174.       // 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
     
175.       signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
     
176. 
     
177.       // 从Awz的角度值反向计算RwGyro的公式请查看网页 http://starlino.com/imu_guide.html
     
178.       for (w=0;w<=1;w++) {
     
179.         RwGyro[0] = sin(Awz[0] * PI / 180);
     
180.         RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
     
181.         RwGyro[1] = sin(Awz[1] * PI / 180);
     
182.         RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
     
183.       }
     
184.       RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
     
185.     }
     
186. 
     
187.     // 把陀螺仪与加速度传感器的值进行结合
     
188.     for (w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);
     
189. 
     
190.     normalize3DVec(RwEst);
     
191.   }
     
192. 
     
193.   firstSample = false;
     
194. }
     
195. 
     
196. 
     
197. void draw() {  
     
198.   getInclination();
     
199. 
     
200.   background(#000000);
     
201.   fill(#ffffff);
     
202. 
     
203.   textFont(font, 20);
     
204.   //float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
     
205.   //text("temp:\n" + temp_decoded + " C", 350, 250);
     
206.   text("RwAcc (G):\n" + RwAcc[0] + "\n" + RwAcc[1] + "\n" + RwAcc[2] + "\ninterval: " + interval, 20, 50);
     
207.   text("Gyro (°/s):\n" + Gyro[0] + "\n" + Gyro[1] + "\n" + Gyro[2], 220, 50);
     
208.   text("Awz (°):\n" + Awz[0] + "\n" + Awz[1], 420, 50);
     
209.   text("RwGyro (°/s):\n" + RwGyro[0] + "\n" + RwGyro[1] + "\n" + RwGyro[2], 20, 180);
     
210.   text("RwEst :\n" + RwEst[0] + "\n" + RwEst[1] + "\n" + RwEst[2], 220, 180);
     
211. 
     
212.   // display axes显示轴
     
213.   pushMatrix();
     
214.   translate(450, 250, 0);
     
215.   stroke(#ffffff);
     
216.   scale(100, 100, 100);
     
217.   line(0, 0, 0, 1, 0, 0);
     
218.   line(0, 0, 0, 0, -1, 0);
     
219.   line(0, 0, 0, 0, 0, 1);
     
220.   line(0, 0, 0, -RwEst[0], RwEst[1], RwEst[2]);
     
221.   popMatrix();
     
222. 
     
223.   drawCube();
     
224. }
     

复制代码

然后点击运行



实验效果：


arduino上的代码主要作用就是采集两个传感器的数据，然后通过串口发送出去。关键的算法在processing上面，代码做了注释，如果想研究可以仔细看看代码。