|
姿态识别应用范围很广,像自平衡车呀,飞行器呀,双足机器人呀之类。本次我们使用Arduino+ITG3205+ADXL345做姿态检测,使用Processing作为输出,实时显示姿态。
本次实验使用的ITG3205与ADXL345都是成品模块,都可以使用I2C接口进行连接。
先看硬件连接,模拟5号口连接I2C模块的SCL,模拟4号口连接I2C模块的SDA口。VCC与GND正常连接,主要不要接错电压,要使用3.3V。I2C模块之间并联。
姿态识别应用范围很广,像自平衡车呀,飞行器呀,双足机器人呀之类。本次我们使用Arduino+ITG3205+ADXL345做姿态检测,使用Processing作为输出,实时显示姿态。
本次实验使用的ITG3205与ADXL345都是成品模块,都可以使用I2C接口进行连接。
先看硬件连接,模拟5号口连接I2C模块的SCL,模拟4号口连接I2C模块的SDA口。VCC与GND正常连接,主要不要接错电压,要使用3.3V。I2C模块之间并联。
2011-11-25 21:09:08 上传下载附件 (124.68 KB)
把下面的代码编译后下载进入arduino控制板中。
00.00.#include <Wire.h> // 调用I2C库
00.
00.// 加速度传感器 ADXL345
00.#define ACC (0x53) //定义ADXL345地址
00.#define A_TO_READ (6) //读取每次占用的字节数 (每个坐标轴占两个字节)
00.
00.
00.// 陀螺仪 ITG3200
00.#define GYRO 0x68 // 定义传感器地址,将AD0连接到GND口,传感器地址为二进制数11101000 (请参考你接口板的原理图)
00.#define G_SMPLRT_DIV 0x15
00.#define G_DLPF_FS 0x16
00.#define G_INT_CFG 0x17
00.#define G_PWR_MGM 0x3E
00.
00.#define G_TO_READ 8 // x,y,z 每个轴2 bytes
00.
00.// 陀螺仪误差修正的偏移量
00.int g_offx = 67;
00.int g_offy = 5;
00.int g_offz = 41;
00.
00.// 加速度传感器误差修正的偏移量
00.int a_offx = -30;
00.int a_offy = -8;
00.int a_offz = 0;
00.
00.char str[512];
00.
00.void initAcc() {
00. //调用 ADXL345
00. writeTo(ACC, 0x2D, 0);
00. writeTo(ACC, 0x2D, 16);
00. writeTo(ACC, 0x2D, 8);
00. //设定在 +-2g 时的默认读数
00.}
00.
00.void getAccelerometerData(int * result) {
00. int regAddress = 0x32; //加速度传感器ADXL345第一轴的数据的设定
00. byte buff[A_TO_READ];
00.
00. readFrom(ACC, regAddress, A_TO_READ, buff); //读取加速度传感器ADXL345的数据
00.
00. //每个轴的读数有10位分辨率,即2个字节.
00. //我们要转换两个bytes为一个int变量
00. result[0] = (((int)buff[1]) << 8) | buff[0] + a_offx;
00. result[1] = (((int)buff[3]) << 8) | buff[2] + a_offy;
00. result[2] = (((int)buff[5]) << 8) | buff[4] + a_offz;
00.}
00.
00.//初始化陀螺仪
00.void initGyro()
00.{
00. /*****************************************
00. * ITG 3200
00. * 电源管理设定:
00. * 时钟选择 =内部振荡器
00. * 无复位, 无睡眠模式
00. * 无待机模式
00. * 采样率 = 125Hz
00. * 参数为+ / - 2000度/秒
00. * 低通滤波器=5HZ
00. * 没有中断
00. ******************************************/
00. writeTo(GYRO, G_PWR_MGM, 0x00);
00. writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF
00. writeTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19
00. writeTo(GYRO, G_INT_CFG, 0x00);
00.}
00.
00.
00.void getGyroscopeData(int * result)
00.{
00. /**************************************
00. * 陀螺仪ITG- 3200的I2C
00. * 寄存器:
00. * temp MSB = 1B, temp LSB = 1C
00. * x axis MSB = 1D, x axis LSB = 1E
00. * y axis MSB = 1F, y axis LSB = 20
00. * z axis MSB = 21, z axis LSB = 22
00. *************************************/
00.
00. int regAddress = 0x1B;
00. int temp, x, y, z;
00. byte buff[G_TO_READ];
00.
00. readFrom(GYRO, regAddress, G_TO_READ, buff); //读取陀螺仪ITG3200的数据
00.
00. result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
00. result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
00. result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
00. result[3] = (buff[0] << 8) | buff[1]; // 温度
00.
00.}
00.
00.
00.void setup()
00.{
00. Serial.begin(9600);
00. Wire.begin();
00. initAcc();
00. initGyro();
00.}
00.
00.
00.void loop()
00.{
00. int acc[3];
00. int gyro[4];
00. getAccelerometerData(acc);
00. getGyroscopeData(gyro);
00.
00. sprintf(str, "%d,%d,%d,%d,%d,%d,%d", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], gyro[3]);
00. Serial.print(str);
00. Serial.print(10, BYTE);
00.
00. //延时50毫秒
00.}
00.
00.
00.//---------------- 功能
00.//将val写入到加速度传感器的地址寄存器中
00.void writeTo(int DEVICE, byte address, byte val) {
00. Wire.beginTransmission(DEVICE); //传送到加速度传感器
00. Wire.send(address); // 发送寄存器地址
00. Wire.send(val); // 发送要写入的值
00. Wire.endTransmission(); //结束传输
00.}
00.
00.
00.//加速度传感器在地址寄存器的缓冲区阵列中读取读数
00.void readFrom(int DEVICE, byte address, int num, byte buff[]) {
00. Wire.beginTransmission(DEVICE); //开始传送至加速度传感器
00. Wire.send(address); //发送读取的地址
00. Wire.endTransmission(); //结束传输
00.
00. Wire.beginTransmission(DEVICE); //开始传送到ACC
00. Wire.requestFrom(DEVICE, num); // 要求从加速度传感器中发送6个字节的数据
00.
00. int i = 0;
00. while(Wire.available()) //当加速度传感器返回的数据小于要求值时(异常情况)
00. {
00. buff = Wire.receive(); // 接收数据
00. i++;
00. }
00. Wire.endTransmission(); //结束传输
00.}
普通浏览复制代码保存代码打印代码
01.#include <Wire.h> // 调用I2C库
02.
03.// 加速度传感器 ADXL345
04.#define ACC (0x53) //定义ADXL345地址
05.#define A_TO_READ (6) //读取每次占用的字节数 (每个坐标轴占两个字节)
06.
07.
08.// 陀螺仪 ITG3200
09.#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 = Wire.receive(); // 接收数据
143. i++;
144. }
145. Wire.endTransmission(); //结束传输
146.}#include <Wire.h> // 调用I2C库<br />
<br />
// 加速度传感器 ADXL345<br />
#define ACC (0x53) //定义ADXL345地址<br />
#define A_TO_READ (6) //读取每次占用的字节数 (每个坐标轴占两个字节)<br />
<br />
<br />
// 陀螺仪 ITG3200 <br />
#define GYRO 0x68 // 定义传感器地址,将AD0连接到GND口,传感器地址为二进制数11101000 (请参考你接口板的原理图)<br />
#define G_SMPLRT_DIV 0x15<br />
#define G_DLPF_FS 0x16<br />
#define G_INT_CFG 0x17<br />
#define G_PWR_MGM 0x3E<br />
<br />
#define G_TO_READ 8 // x,y,z 每个轴2 bytes<br />
<br />
// 陀螺仪误差修正的偏移量 <br />
int g_offx = 67;<br />
int g_offy = 5;<br />
int g_offz = 41;<br />
<br />
// 加速度传感器误差修正的偏移量<br />
int a_offx = -30;<br />
int a_offy = -8;<br />
int a_offz = 0;<br />
<br />
char str[512]; <br />
<br />
void initAcc() {<br />
//调用 ADXL345<br />
writeTo(ACC, 0x2D, 0); <br />
writeTo(ACC, 0x2D, 16);<br />
writeTo(ACC, 0x2D, 8);<br />
//设定在 +-2g 时的默认读数<br />
}<br />
<br />
void getAccelerometerData(int * result) {<br />
int regAddress = 0x32; //加速度传感器ADXL345第一轴的数据的设定<br />
byte buff[A_TO_READ];<br />
<br />
readFrom(ACC, regAddress, A_TO_READ, buff); //读取加速度传感器ADXL345的数据<br />
<br />
//每个轴的读数有10位分辨率,即2个字节. <br />
//我们要转换两个bytes为一个int变量<br />
result[0] = (((int)buff[1]) << 8) | buff[0] + a_offx; <br />
result[1] = (((int)buff[3]) << 8) | buff[2] + a_offy;<br />
result[2] = (((int)buff[5]) << 8) | buff[4] + a_offz;<br />
}<br />
<br />
//初始化陀螺仪<br />
void initGyro()<br />
{<br />
/*****************************************<br />
* ITG 3200<br />
* 电源管理设定:<br />
* 时钟选择 =内部振荡器<br />
* 无复位, 无睡眠模式<br />
* 无待机模式<br />
* 采样率 = 125Hz<br />
* 参数为+ / - 2000度/秒<br />
* 低通滤波器=5HZ<br />
* 没有中断<br />
******************************************/<br />
writeTo(GYRO, G_PWR_MGM, 0x00);<br />
writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF<br />
writeTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19<br />
writeTo(GYRO, G_INT_CFG, 0x00);<br />
}<br />
<br />
<br />
void getGyroscopeData(int * result)<br />
{<br />
/**************************************<br />
* 陀螺仪ITG- 3200的I2C<br />
* 寄存器:<br />
* temp MSB = 1B, temp LSB = 1C<br />
* x axis MSB = 1D, x axis LSB = 1E<br />
* y axis MSB = 1F, y axis LSB = 20<br />
* z axis MSB = 21, z axis LSB = 22<br />
*************************************/<br />
<br />
int regAddress = 0x1B;<br />
int temp, x, y, z;<br />
byte buff[G_TO_READ];<br />
<br />
readFrom(GYRO, regAddress, G_TO_READ, buff); //读取陀螺仪ITG3200的数据<br />
<br />
result[0] = ((buff[2] << 8) | buff[3]) + g_offx;<br />
result[1] = ((buff[4] << 8) | buff[5]) + g_offy;<br />
result[2] = ((buff[6] << 8) | buff[7]) + g_offz;<br />
result[3] = (buff[0] << 8) | buff[1]; // 温度<br />
<br />
}<br />
<br />
<br />
void setup()<br />
{<br />
Serial.begin(9600);<br />
Wire.begin();<br />
initAcc();<br />
initGyro();<br />
}<br />
<br />
<br />
void loop()<br />
{<br />
int acc[3];<br />
int gyro[4];<br />
getAccelerometerData(acc);<br />
getGyroscopeData(gyro);<br />
<br />
sprintf(str, "%d,%d,%d,%d,%d,%d,%d", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], gyro[3]); <br />
Serial.print(str);<br />
Serial.print(10, BYTE);<br />
<br />
//延时50毫秒<br />
}<br />
<br />
<br />
//---------------- 功能<br />
//将val写入到加速度传感器的地址寄存器中<br />
void writeTo(int DEVICE, byte address, byte val) {<br />
Wire.beginTransmission(DEVICE); //传送到加速度传感器<br />
Wire.send(address); // 发送寄存器地址<br />
Wire.send(val); // 发送要写入的值<br />
Wire.endTransmission(); //结束传输<br />
}<br />
<br />
<br />
//加速度传感器在地址寄存器的缓冲区阵列中读取读数<br />
void readFrom(int DEVICE, byte address, int num, byte buff[]) {<br />
Wire.beginTransmission(DEVICE); //开始传送至加速度传感器 <br />
Wire.send(address); //发送读取的地址<br />
Wire.endTransmission(); //结束传输<br />
<br />
Wire.beginTransmission(DEVICE); //开始传送到ACC<br />
Wire.requestFrom(DEVICE, num); // 要求从加速度传感器中发送6个字节的数据<br />
<br />
int i = 0;<br />
while(Wire.available()) //当加速度传感器返回的数据小于要求值时(异常情况)<br />
{ <br />
buff = Wire.receive(); // 接收数据<br />
i++;<br />
}<br />
Wire.endTransmission(); //结束传输<br />
}
先介绍一下processing的基本使用方法,先从http://processing.org/download/下载回来processing的IDE。
然后把下面代码拷贝进入进入processing,查看连接arduino的com口是第几个。根据具体情况调整com口连接代码。
00.00.import processing.serial.*;
00.
00.Serial myPort; // 创建串口对象myPort
00.
00.boolean firstSample = true;
00.
00.float [] RwAcc = new float[3]; // 通过加速度传感器把重力加速度投影在x/y/z三轴上
00.float [] Gyro = new float[3]; // 陀螺仪读取
00.float [] RwGyro = new float[3]; // 重新读取陀螺仪
00.float [] Awz = new float[2]; // XZ/ YZ平面和Z轴(度)R的投影之间的角度
00.float [] RwEst = new float[3];
00.
00.
00.int lastTime = 0;
00.int interval = 0;
00.float wGyro = 10.0;
00.
00.int lf = 10; // 10在ASCII表中表示'\n'
00.byte[] inBuffer = new byte[100];
00.
00.PFont font;
00.final int VIEW_SIZE_X = 600, VIEW_SIZE_Y = 600;
00.
00.
00.void setup()
00.{
00. size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
00. myPort = new Serial(this, Serial.list()[2], 9600); // 设置电脑第三个COM口为连接端口,这个要根据你电脑情况进行设置。
00.
00. //myPort = new Serial(this, "/dev/ttyUSB0", 9600);
00.
00. // 加载字体,字体必须在代码文件同目录下的data文件夹中
00. font = loadFont("CourierNew36.vlw");
00.}
00.
00.
00.void readSensors() {
00. if (myPort.available() > 0) {
00. if (myPort.readBytesUntil(lf, inBuffer) > 0) {
00. String inputString = new String(inBuffer);
00. String [] inputStringArr = split(inputString, ',');
00.
00. // 把原始数据转换为G
00. RwAcc[0] = float(inputStringArr[0]) / 256.0;
00. RwAcc[1] = float(inputStringArr[1])/ 256.0;
00. RwAcc[2] = float(inputStringArr[2])/ 256.0;
00.
00. // 把原始数据转换为"度/秒"
00. Gyro[0] = float(inputStringArr[3]) / 14.375;
00. Gyro[1] = float(inputStringArr[4]) / 14.375;
00. Gyro[2] = float(inputStringArr[5]) / 14.375;
00. }
00. }
00.}
00.
00.
00.void normalize3DVec(float [] vector) {
00. float R;
00. R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
00. vector[0] /= R;
00. vector[1] /= R;
00. vector[2] /= R;
00.}
00.
00.
00.float squared(float x) {
00. return x*x;
00.}
00.
00.
00.void buildBoxShape() {
00. //box(60, 10, 40);
00. noStroke();
00. beginShape(QUADS);
00.
00. //Z+ (绘图区域)
00. fill(#00ff00);
00. vertex(-30, -5, 20);
00. vertex(30, -5, 20);
00. vertex(30, 5, 20);
00. vertex(-30, 5, 20);
00.
00. //Z-
00. fill(#0000ff);
00. vertex(-30, -5, -20);
00. vertex(30, -5, -20);
00. vertex(30, 5, -20);
00. vertex(-30, 5, -20);
00.
00. //X-
00. fill(#ff0000);
00. vertex(-30, -5, -20);
00. vertex(-30, -5, 20);
00. vertex(-30, 5, 20);
00. vertex(-30, 5, -20);
00.
00. //X+
00. fill(#ffff00);
00. vertex(30, -5, -20);
00. vertex(30, -5, 20);
00. vertex(30, 5, 20);
00. vertex(30, 5, -20);
00.
00. //Y-
00. fill(#ff00ff);
00. vertex(-30, -5, -20);
00. vertex(30, -5, -20);
00. vertex(30, -5, 20);
00. vertex(-30, -5, 20);
00.
00. //Y+
00. fill(#00ffff);
00. vertex(-30, 5, -20);
00. vertex(30, 5, -20);
00. vertex(30, 5, 20);
00. vertex(-30, 5, 20);
00.
00. endShape();
00.}
00.
00.
00.void drawCube() {
00. pushMatrix();
00. translate(300, 450, 0);
00. scale(4, 4, 4);
00.
00. rotateX(HALF_PI * -RwEst[0]);
00. rotateZ(HALF_PI * RwEst[1]);
00.
00. buildBoxShape();
00.
00. popMatrix();
00.}
00.
00.
00.void getInclination() {
00. int w = 0;
00. float tmpf = 0.0;
00. int currentTime, signRzGyro;
00.
00.
00. readSensors();
00. normalize3DVec(RwAcc);
00.
00. currentTime = millis();
00. interval = currentTime - lastTime;
00. lastTime = currentTime;
00.
00. if (firstSample || Float.isNaN(RwEst[0])) { // NaN用来等待检查从arduino过来的数据
00. for (w=0;w<=2;w++) {
00. RwEst[w] = RwAcc[w]; // 初始化加速度传感器读数
00. }
00. }
00. else {
00. // 对RwGyro进行评估
00. if (abs(RwEst[2]) < 0.1) {
00. // Rz值非常的小,它的作用是作为Axz与Ayz的计算参照值,防止放大的波动产生错误的结果。
00. // 这种情况下就跳过当前的陀螺仪数据,使用以前的。
00. for (w=0;w<=2;w++) {
00. RwGyro[w] = RwEst[w];
00. }
00. }
00. else {
00. // ZX/ZY平面和Z轴R的投影之间的角度,基于最近一次的RwEst值
00. for (w=0;w<=1;w++) {
00. tmpf = Gyro[w]; // 获取当前陀螺仪的deg/s
00. tmpf *= interval / 1000.0f; // 得到角度变化值
00. Awz[w] = atan2(RwEst[w], RwEst[2]) * 180 / PI; // 得到角度并转换为度
00. Awz[w] += tmpf; // 根据陀螺仪的运动得到更新后的角度
00. }
00.
00. // 判断RzGyro是多少,主要看Axz的弧度是多少
00. // 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
00. signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
00.
00. // 从Awz的角度值反向计算RwGyro的公式请查看网页 http://starlino.com/imu_guide.html
00. for (w=0;w<=1;w++) {
00. RwGyro[0] = sin(Awz[0] * PI / 180);
00. RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
00. RwGyro[1] = sin(Awz[1] * PI / 180);
00. RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
00. }
00. RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
00. }
00.
00. // 把陀螺仪与加速度传感器的值进行结合
00. for (w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);
00.
00. normalize3DVec(RwEst);
00. }
00.
00. firstSample = false;
00.}
00.
00.
00.void draw() {
00. getInclination();
00.
00. background(#000000);
00. fill(#ffffff);
00.
00. textFont(font, 20);
00. //float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
00. //text("temp:\n" + temp_decoded + " C", 350, 250);
00. text("RwAcc (G):\n" + RwAcc[0] + "\n" + RwAcc[1] + "\n" + RwAcc[2] + "\ninterval: " + interval, 20, 50);
00. text("Gyro (°/s):\n" + Gyro[0] + "\n" + Gyro[1] + "\n" + Gyro[2], 220, 50);
00. text("Awz (°):\n" + Awz[0] + "\n" + Awz[1], 420, 50);
00. text("RwGyro (°/s):\n" + RwGyro[0] + "\n" + RwGyro[1] + "\n" + RwGyro[2], 20, 180);
00. text("RwEst :\n" + RwEst[0] + "\n" + RwEst[1] + "\n" + RwEst[2], 220, 180);
00.
00. // display axes显示轴
00. pushMatrix();
00. translate(450, 250, 0);
00. stroke(#ffffff);
00. scale(100, 100, 100);
00. line(0, 0, 0, 1, 0, 0);
00. line(0, 0, 0, 0, -1, 0);
00. line(0, 0, 0, 0, 0, 1);
00. line(0, 0, 0, -RwEst[0], RwEst[1], RwEst[2]);
00. popMatrix();
00.
00. drawCube();
00.}
普通浏览复制代码保存代码打印代码
01.import processing.serial.*;
02.
03.Serial myPort; // 创建串口对象myPort
04.
05.boolean firstSample = true;
06.
07.float [] RwAcc = new float[3]; // 通过加速度传感器把重力加速度投影在x/y/z三轴上
08.float [] Gyro = new float[3]; // 陀螺仪读取
09.float [] RwGyro = new float[3]; // 重新读取陀螺仪
10.float [] Awz = new float[2]; // XZ/ YZ平面和Z轴(度)R的投影之间的角度
11.float [] RwEst = new float[3];
12.
13.
14.int lastTime = 0;
15.int interval = 0;
16.float wGyro = 10.0;
17.
18.int lf = 10; // 10在ASCII表中表示'\n'
19.byte[] inBuffer = new byte[100];
20.
21.PFont font;
22.final int VIEW_SIZE_X = 600, VIEW_SIZE_Y = 600;
23.
24.
25.void setup()
26.{
27. size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
28. myPort = new Serial(this, Serial.list()[2], 9600); // 设置电脑第三个COM口为连接端口,这个要根据你电脑情况进行设置。
29.
30. //myPort = new Serial(this, "/dev/ttyUSB0", 9600);
31.
32. // 加载字体,字体必须在代码文件同目录下的data文件夹中
33. font = loadFont("CourierNew36.vlw");
34.}
35.
36.
37.void readSensors() {
38. if (myPort.available() > 0) {
39. if (myPort.readBytesUntil(lf, inBuffer) > 0) {
40. String inputString = new String(inBuffer);
41. String [] inputStringArr = split(inputString, ',');
42.
43. // 把原始数据转换为G
44. RwAcc[0] = float(inputStringArr[0]) / 256.0;
45. RwAcc[1] = float(inputStringArr[1])/ 256.0;
46. RwAcc[2] = float(inputStringArr[2])/ 256.0;
47.
48. // 把原始数据转换为"度/秒"
49. Gyro[0] = float(inputStringArr[3]) / 14.375;
50. Gyro[1] = float(inputStringArr[4]) / 14.375;
51. Gyro[2] = float(inputStringArr[5]) / 14.375;
52. }
53. }
54.}
55.
56.
57.void normalize3DVec(float [] vector) {
58. float R;
59. R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
60. vector[0] /= R;
61. vector[1] /= R;
62. vector[2] /= R;
63.}
64.
65.
66.float squared(float x) {
67. return x*x;
68.}
69.
70.
71.void buildBoxShape() {
72. //box(60, 10, 40);
73. noStroke();
74. beginShape(QUADS);
75.
76. //Z+ (绘图区域)
77. fill(#00ff00);
78. vertex(-30, -5, 20);
79. vertex(30, -5, 20);
80. vertex(30, 5, 20);
81. vertex(-30, 5, 20);
82.
83. //Z-
84. fill(#0000ff);
85. vertex(-30, -5, -20);
86. vertex(30, -5, -20);
87. vertex(30, 5, -20);
88. vertex(-30, 5, -20);
89.
90. //X-
91. fill(#ff0000);
92. vertex(-30, -5, -20);
93. vertex(-30, -5, 20);
94. vertex(-30, 5, 20);
95. vertex(-30, 5, -20);
96.
97. //X+
98. fill(#ffff00);
99. vertex(30, -5, -20);
100. vertex(30, -5, 20);
101. vertex(30, 5, 20);
102. vertex(30, 5, -20);
103.
104. //Y-
105. fill(#ff00ff);
106. vertex(-30, -5, -20);
107. vertex(30, -5, -20);
108. vertex(30, -5, 20);
109. vertex(-30, -5, 20);
110.
111. //Y+
112. fill(#00ffff);
113. vertex(-30, 5, -20);
114. vertex(30, 5, -20);
115. vertex(30, 5, 20);
116. vertex(-30, 5, 20);
117.
118. endShape();
119.}
120.
121.
122.void drawCube() {
123. pushMatrix();
124. translate(300, 450, 0);
125. scale(4, 4, 4);
126.
127. rotateX(HALF_PI * -RwEst[0]);
128. rotateZ(HALF_PI * RwEst[1]);
129.
130. buildBoxShape();
131.
132. popMatrix();
133.}
134.
135.
136.void getInclination() {
137. int w = 0;
138. float tmpf = 0.0;
139. int currentTime, signRzGyro;
140.
141.
142. readSensors();
143. normalize3DVec(RwAcc);
144.
145. currentTime = millis();
146. interval = currentTime - lastTime;
147. lastTime = currentTime;
148.
149. if (firstSample || Float.isNaN(RwEst[0])) { // NaN用来等待检查从arduino过来的数据
150. for (w=0;w<=2;w++) {
151. RwEst[w] = RwAcc[w]; // 初始化加速度传感器读数
152. }
153. }
154. else {
155. // 对RwGyro进行评估
156. if (abs(RwEst[2]) < 0.1) {
157. // Rz值非常的小,它的作用是作为Axz与Ayz的计算参照值,防止放大的波动产生错误的结果。
158. // 这种情况下就跳过当前的陀螺仪数据,使用以前的。
159. for (w=0;w<=2;w++) {
160. RwGyro[w] = RwEst[w];
161. }
162. }
163. else {
164. // ZX/ZY平面和Z轴R的投影之间的角度,基于最近一次的RwEst值
165. for (w=0;w<=1;w++) {
166. tmpf = Gyro[w]; // 获取当前陀螺仪的deg/s
167. tmpf *= interval / 1000.0f; // 得到角度变化值
168. Awz[w] = atan2(RwEst[w], RwEst[2]) * 180 / PI; // 得到角度并转换为度
169. Awz[w] += tmpf; // 根据陀螺仪的运动得到更新后的角度
170. }
171.
172. // 判断RzGyro是多少,主要看Axz的弧度是多少
173. // 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
174. signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
175.
176. // 从Awz的角度值反向计算RwGyro的公式请查看网页 http://starlino.com/imu_guide.html
177. for (w=0;w<=1;w++) {
178. RwGyro[0] = sin(Awz[0] * PI / 180);
179. RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
180. RwGyro[1] = sin(Awz[1] * PI / 180);
181. RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
182. }
183. RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
184. }
185.
186. // 把陀螺仪与加速度传感器的值进行结合
187. for (w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);
188.
189. normalize3DVec(RwEst);
190. }
191.
192. firstSample = false;
193.}
194.
195.
196.void draw() {
197. getInclination();
198.
199. background(#000000);
200. fill(#ffffff);
201.
202. textFont(font, 20);
203. //float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
204. //text("temp:\n" + temp_decoded + " C", 350, 250);
205. text("RwAcc (G):\n" + RwAcc[0] + "\n" + RwAcc[1] + "\n" + RwAcc[2] + "\ninterval: " + interval, 20, 50);
206. text("Gyro (°/s):\n" + Gyro[0] + "\n" + Gyro[1] + "\n" + Gyro[2], 220, 50);
207. text("Awz (°):\n" + Awz[0] + "\n" + Awz[1], 420, 50);
208. text("RwGyro (°/s):\n" + RwGyro[0] + "\n" + RwGyro[1] + "\n" + RwGyro[2], 20, 180);
209. text("RwEst :\n" + RwEst[0] + "\n" + RwEst[1] + "\n" + RwEst[2], 220, 180);
210.
211. // display axes显示轴
212. pushMatrix();
213. translate(450, 250, 0);
214. stroke(#ffffff);
215. scale(100, 100, 100);
216. line(0, 0, 0, 1, 0, 0);
217. line(0, 0, 0, 0, -1, 0);
218. line(0, 0, 0, 0, 0, 1);
219. line(0, 0, 0, -RwEst[0], RwEst[1], RwEst[2]);
220. popMatrix();
221.
222. drawCube();
223.}
详情可到以下网站
arduino学习笔记28 - ITG3200 ADXL345做姿态识别实验
http://geek-workshop.com/forum.php?mod=viewthread&tid=236 |
阿莫论坛20周年了!感谢大家的支持与爱护!!
知道什么是神吗?其实神本来也是人,只不过神做了人做不到的事情 所以才成了神。 (头文字D, 杜汶泽)
|