arduino学习笔记28 - ITG3200 ADXL345做姿态识别实验四轴利器(一) (amobbs.com 阿莫电子技术论坛) -

andy 发表于 2011-12-28 21:31:02

arduino学习笔记28 - ITG3200 ADXL345做姿态识别实验四轴利器(一)

姿态识别应用范围很广，像自平衡车呀，飞行器呀，双足机器人呀之类。本次我们使用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;
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;
00.
00.readFrom(ACC, regAddress, A_TO_READ, buff); //读取加速度传感器ADXL345的数据
00.
00.//每个轴的读数有10位分辨率，即2个字节.
00.//我们要转换两个bytes为一个int变量
00.result = (((int)buff) << 8) | buff + a_offx;
00.result = (((int)buff) << 8) | buff + a_offy;
00.result = (((int)buff) << 8) | buff + 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;
00.
00.readFrom(GYRO, regAddress, G_TO_READ, buff); //读取陀螺仪ITG3200的数据
00.
00.result = ((buff << 8) | buff) + g_offx;
00.result = ((buff << 8) | buff) + g_offy;
00.result = ((buff << 8) | buff) + g_offz;
00.result = (buff << 8) | buff; // 温度
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;
00.int gyro;
00.getAccelerometerData(acc);
00.getGyroscopeData(gyro);
00.
00.sprintf(str, "%d,%d,%d,%d,%d,%d,%d", acc, acc, acc, gyro, gyro, gyro, gyro);
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;
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;
40.
41.readFrom(ACC, regAddress, A_TO_READ, buff); //读取加速度传感器ADXL345的数据
42.
43.//每个轴的读数有10位分辨率，即2个字节.
44.//我们要转换两个bytes为一个int变量
45.result = (((int)buff) << 8) | buff + a_offx;
46.result = (((int)buff) << 8) | buff + a_offy;
47.result = (((int)buff) << 8) | buff + 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;
85.
86.readFrom(GYRO, regAddress, G_TO_READ, buff); //读取陀螺仪ITG3200的数据
87.
88.result = ((buff << 8) | buff) + g_offx;
89.result = ((buff << 8) | buff) + g_offy;
90.result = ((buff << 8) | buff) + g_offz;
91.result = (buff << 8) | buff; // 温度
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;
108.int gyro;
109.getAccelerometerData(acc);
110.getGyroscopeData(gyro);
111.
112.sprintf(str, "%d,%d,%d,%d,%d,%d,%d", acc, acc, acc, gyro, gyro, gyro, gyro);
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库 
 
// 加速度传感器 ADXL345 
#define ACC (0x53) //定义ADXL345地址 
#define A_TO_READ (6) //读取每次占用的字节数 (每个坐标轴占两个字节) 
 
 
// 陀螺仪 ITG3200 
#define GYRO 0x68 // 定义传感器地址,将AD0连接到GND口，传感器地址为二进制数11101000 (请参考你接口板的原理图) 
#define G_SMPLRT_DIV 0x15 
#define G_DLPF_FS 0x16 
#define G_INT_CFG 0x17 
#define G_PWR_MGM 0x3E 
 
#define G_TO_READ 8 // x,y,z 每个轴2 bytes 
 
// 陀螺仪误差修正的偏移量 
int g_offx = 67; 
int g_offy = 5; 
int g_offz = 41; 
 
// 加速度传感器误差修正的偏移量 
int a_offx = -30; 
int a_offy = -8; 
int a_offz = 0; 
 
char str; 
 
void initAcc() { 
//调用 ADXL345 
writeTo(ACC, 0x2D, 0); 
writeTo(ACC, 0x2D, 16); 
writeTo(ACC, 0x2D, 8); 
//设定在 +-2g 时的默认读数 
} 
 
void getAccelerometerData(int * result) { 
int regAddress = 0x32; //加速度传感器ADXL345第一轴的数据的设定 
byte buff; 
 
readFrom(ACC, regAddress, A_TO_READ, buff); //读取加速度传感器ADXL345的数据 
 
//每个轴的读数有10位分辨率，即2个字节. 
//我们要转换两个bytes为一个int变量 
result = (((int)buff) << 8) | buff + a_offx; 
result = (((int)buff) << 8) | buff + a_offy; 
result = (((int)buff) << 8) | buff + a_offz; 
} 
 
//初始化陀螺仪 
void initGyro() 
{ 
/***************************************** 
* ITG 3200 
* 电源管理设定： 
* 时钟选择 =内部振荡器 
* 无复位, 无睡眠模式 
* 无待机模式 
* 采样率 = 125Hz 
* 参数为+ / - 2000度/秒 
* 低通滤波器=5HZ 
* 没有中断 
******************************************/ 
writeTo(GYRO, G_PWR_MGM, 0x00); 
writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF 
writeTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19 
writeTo(GYRO, G_INT_CFG, 0x00); 
} 
 
 
void getGyroscopeData(int * result) 
{ 
/************************************** 
* 陀螺仪ITG- 3200的I2C 
* 寄存器： 
* temp MSB = 1B, temp LSB = 1C 
* x axis MSB = 1D, x axis LSB = 1E 
* y axis MSB = 1F, y axis LSB = 20 
* z axis MSB = 21, z axis LSB = 22 
*************************************/ 
 
int regAddress = 0x1B; 
int temp, x, y, z; 
byte buff; 
 
readFrom(GYRO, regAddress, G_TO_READ, buff); //读取陀螺仪ITG3200的数据 
 
result = ((buff << 8) | buff) + g_offx; 
result = ((buff << 8) | buff) + g_offy; 
result = ((buff << 8) | buff) + g_offz; 
result = (buff << 8) | buff; // 温度 
 
} 
 
 
void setup() 
{ 
Serial.begin(9600); 
Wire.begin(); 
initAcc(); 
initGyro(); 
} 
 
 
void loop() 
{ 
int acc; 
int gyro; 
getAccelerometerData(acc); 
getGyroscopeData(gyro); 
 
sprintf(str, "%d,%d,%d,%d,%d,%d,%d", acc, acc, acc, gyro, gyro, gyro, gyro); 
Serial.print(str); 
Serial.print(10, BYTE); 
 
//延时50毫秒 
} 
 
 
//---------------- 功能 
//将val写入到加速度传感器的地址寄存器中 
void writeTo(int DEVICE, byte address, byte val) { 
Wire.beginTransmission(DEVICE); //传送到加速度传感器 
Wire.send(address); // 发送寄存器地址 
Wire.send(val); // 发送要写入的值 
Wire.endTransmission(); //结束传输 
} 
 
 
//加速度传感器在地址寄存器的缓冲区阵列中读取读数 
void readFrom(int DEVICE, byte address, int num, byte buff[]) { 
Wire.beginTransmission(DEVICE); //开始传送至加速度传感器 
Wire.send(address); //发送读取的地址 
Wire.endTransmission(); //结束传输 
 
Wire.beginTransmission(DEVICE); //开始传送到ACC 
Wire.requestFrom(DEVICE, num); // 要求从加速度传感器中发送6个字节的数据 
 
int i = 0; 
while(Wire.available()) //当加速度传感器返回的数据小于要求值时（异常情况） 
{ 
buff = Wire.receive(); // 接收数据 
i++; 
} 
Wire.endTransmission(); //结束传输 
}
先介绍一下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; // 通过加速度传感器把重力加速度投影在x/y/z三轴上
00.float [] Gyro = new float; // 陀螺仪读取
00.float [] RwGyro = new float; // 重新读取陀螺仪
00.float [] Awz = new float; // XZ/ YZ平面和Z轴（度）R的投影之间的角度
00.float [] RwEst = new float;
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;
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(), 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 = float(inputStringArr) / 256.0;
00. RwAcc = float(inputStringArr)/ 256.0;
00. RwAcc = float(inputStringArr)/ 256.0;
00.
00. // 把原始数据转换为"度/秒"
00. Gyro = float(inputStringArr) / 14.375;
00. Gyro = float(inputStringArr) / 14.375;
00. Gyro = float(inputStringArr) / 14.375;
00. }
00.}
00.}
00.
00.
00.void normalize3DVec(float [] vector) {
00.float R;
00.R = sqrt(vector*vector + vector*vector + vector*vector);
00.vector /= R;
00.vector /= R;
00.vector /= 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);
00.rotateZ(HALF_PI * RwEst);
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)) { // NaN用来等待检查从arduino过来的数据
00. for (w=0;w<=2;w++) {
00. RwEst = RwAcc; // 初始化加速度传感器读数
00. }
00.}
00.else {
00. // 对RwGyro进行评估
00. if (abs(RwEst) < 0.1) {
00. // Rz值非常的小，它的作用是作为Axz与Ayz的计算参照值，防止放大的波动产生错误的结果。
00. // 这种情况下就跳过当前的陀螺仪数据，使用以前的。
00. for (w=0;w<=2;w++) {
00. RwGyro = RwEst;
00. }
00. }
00. else {
00. // ZX/ZY平面和Z轴R的投影之间的角度，基于最近一次的RwEst值
00. for (w=0;w<=1;w++) {
00. tmpf = Gyro; // 获取当前陀螺仪的deg/s
00. tmpf *= interval / 1000.0f; // 得到角度变化值
00. Awz = atan2(RwEst, RwEst) * 180 / PI; // 得到角度并转换为度
00. Awz += tmpf; // 根据陀螺仪的运动得到更新后的角度
00. }
00.
00. // 判断RzGyro是多少，主要看Axz的弧度是多少
00. // 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
00. signRzGyro = ( cos(Awz * PI / 180) >=0 ) ? 1 : -1;
00.
00. // 从Awz的角度值反向计算RwGyro的公式请查看网页 http://starlino.com/imu_guide.html
00. for (w=0;w<=1;w++) {
00. RwGyro = sin(Awz * PI / 180);
00. RwGyro /= sqrt( 1 + squared(cos(Awz * PI / 180)) * squared(tan(Awz * PI / 180)) );
00. RwGyro = sin(Awz * PI / 180);
00. RwGyro /= sqrt( 1 + squared(cos(Awz * PI / 180)) * squared(tan(Awz * PI / 180)) );
00. }
00. RwGyro = signRzGyro * sqrt(1 - squared(RwGyro) - squared(RwGyro));
00. }
00.
00. // 把陀螺仪与加速度传感器的值进行结合
00. for (w=0;w<=2;w++) RwEst = (RwAcc + wGyro * RwGyro) / (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 + "\n" + RwAcc + "\n" + RwAcc + "\ninterval: " + interval, 20, 50);
00.text("Gyro (°/s):\n" + Gyro + "\n" + Gyro + "\n" + Gyro, 220, 50);
00.text("Awz (°):\n" + Awz + "\n" + Awz, 420, 50);
00.text("RwGyro (°/s):\n" + RwGyro + "\n" + RwGyro + "\n" + RwGyro, 20, 180);
00.text("RwEst :\n" + RwEst + "\n" + RwEst + "\n" + RwEst, 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, RwEst, RwEst);
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; // 通过加速度传感器把重力加速度投影在x/y/z三轴上
08.float [] Gyro = new float; // 陀螺仪读取
09.float [] RwGyro = new float; // 重新读取陀螺仪
10.float [] Awz = new float; // XZ/ YZ平面和Z轴（度）R的投影之间的角度
11.float [] RwEst = new float;
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;
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(), 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 = float(inputStringArr) / 256.0;
45. RwAcc = float(inputStringArr)/ 256.0;
46. RwAcc = float(inputStringArr)/ 256.0;
47.
48. // 把原始数据转换为"度/秒"
49. Gyro = float(inputStringArr) / 14.375;
50. Gyro = float(inputStringArr) / 14.375;
51. Gyro = float(inputStringArr) / 14.375;
52. }
53.}
54.}
55.
56.
57.void normalize3DVec(float [] vector) {
58.float R;
59.R = sqrt(vector*vector + vector*vector + vector*vector);
60.vector /= R;
61.vector /= R;
62.vector /= 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);
128.rotateZ(HALF_PI * RwEst);
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)) { // NaN用来等待检查从arduino过来的数据
150. for (w=0;w<=2;w++) {
151. RwEst = RwAcc; // 初始化加速度传感器读数
152. }
153.}
154.else {
155. // 对RwGyro进行评估
156. if (abs(RwEst) < 0.1) {
157. // Rz值非常的小，它的作用是作为Axz与Ayz的计算参照值，防止放大的波动产生错误的结果。
158. // 这种情况下就跳过当前的陀螺仪数据，使用以前的。
159. for (w=0;w<=2;w++) {
160. RwGyro = RwEst;
161. }
162. }
163. else {
164. // ZX/ZY平面和Z轴R的投影之间的角度，基于最近一次的RwEst值
165. for (w=0;w<=1;w++) {
166. tmpf = Gyro; // 获取当前陀螺仪的deg/s
167. tmpf *= interval / 1000.0f; // 得到角度变化值
168. Awz = atan2(RwEst, RwEst) * 180 / PI; // 得到角度并转换为度
169. Awz += tmpf; // 根据陀螺仪的运动得到更新后的角度
170. }
171.
172. // 判断RzGyro是多少，主要看Axz的弧度是多少
173. // 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
174. signRzGyro = ( cos(Awz * PI / 180) >=0 ) ? 1 : -1;
175.
176. // 从Awz的角度值反向计算RwGyro的公式请查看网页 http://starlino.com/imu_guide.html
177. for (w=0;w<=1;w++) {
178. RwGyro = sin(Awz * PI / 180);
179. RwGyro /= sqrt( 1 + squared(cos(Awz * PI / 180)) * squared(tan(Awz * PI / 180)) );
180. RwGyro = sin(Awz * PI / 180);
181. RwGyro /= sqrt( 1 + squared(cos(Awz * PI / 180)) * squared(tan(Awz * PI / 180)) );
182. }
183. RwGyro = signRzGyro * sqrt(1 - squared(RwGyro) - squared(RwGyro));
184. }
185.
186. // 把陀螺仪与加速度传感器的值进行结合
187. for (w=0;w<=2;w++) RwEst = (RwAcc + wGyro * RwGyro) / (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 + "\n" + RwAcc + "\n" + RwAcc + "\ninterval: " + interval, 20, 50);
206.text("Gyro (°/s):\n" + Gyro + "\n" + Gyro + "\n" + Gyro, 220, 50);
207.text("Awz (°):\n" + Awz + "\n" + Awz, 420, 50);
208.text("RwGyro (°/s):\n" + RwGyro + "\n" + RwGyro + "\n" + RwGyro, 20, 180);
209.text("RwEst :\n" + RwEst + "\n" + RwEst + "\n" + RwEst, 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, RwEst, RwEst);
220.popMatrix();
221.
222.drawCube();
223.}

详情可到以下网站

arduino学习笔记28 - ITG3200 ADXL345做姿态识别实验
http://geek-workshop.com/forum.php?mod=viewthread&tid=236

pitolan 发表于 2011-12-28 21:41:50

顶

pp786702237 发表于 2011-12-28 22:05:04

顶…

frank_li 发表于 2011-12-30 21:44:32

太酷了，

dujun168 发表于 2012-1-5 16:26:32

DING

maxuedong 发表于 2012-1-6 21:57:52

这代码看得有点晕啊

HYZ1989 发表于 2012-1-21 19:02:11

mark

FpvCamera 发表于 2012-1-21 20:11:18

最近这类有技术含量的帖子太少了，顶起来

FpvCamera 发表于 2012-1-21 20:14:26

最近这类有技术含量的帖子太少了，顶起来

ggyyll8683 发表于 2012-1-21 20:19:25

好东西，mark

redwolf310 发表于 2012-2-3 23:41:54

mark

robotkid 发表于 2012-2-4 09:34:42

有含量，楼主不如写流程，我来转成STM32。我也有一样的传感硬件

ledatou 发表于 2012-2-4 10:47:13

最近这类有技术含量的帖子太少了，顶起来

00661209 发表于 2012-2-11 22:36:48

学习了

Name_006 发表于 2012-2-12 23:14:04

学习一下

kimo 发表于 2012-2-29 21:09:37

学习贴 mark

wenwu 发表于 2012-3-1 00:26:41

mark

feiling208 发表于 2012-3-2 08:41:54

mark

haigerl 发表于 2012-3-2 09:05:03

mark

kdaiee 发表于 2012-3-4 09:06:46

谢谢楼主

kdaiee 发表于 2012-3-4 09:06:57

谢谢楼主

deadline2012 发表于 2012-3-4 11:02:54

马克

wuchengjun 发表于 2012-3-4 20:20:57

haobu cuo

lxl_lw 发表于 2012-4-9 09:35:58

好帖子，顶一个先。

seazhui 发表于 2012-4-9 11:27:04

国内arduino貌似很弱啊。。。。一个朋友说国外这个超级火的。。。

Randy1022 发表于 2012-4-12 09:50:34

我也在玩ITG3200+ADXL345，不过不是很懂！

梦回大唐 发表于 2012-7-24 18:04:42

学习一下。

GIKE 发表于 2012-8-2 21:29:35

标记，好东西

wubingqp 发表于 2012-8-3 13:15:47

学习贴MARK

ksh84222 发表于 2012-8-4 23:24:02

不错！收藏了！

Fulai 发表于 2012-8-7 15:36:50

技术贴，顶一下~~~~

默默七 发表于 2012-8-23 09:53:18

不错。用processing是亮点

jordonwu 发表于 2012-8-23 10:47:36

学习了,mark

jokemcu 发表于 2012-8-23 17:16:01

表示没看懂，楼上顶的难道都看懂了、、、、、

sz_works 发表于 2012-9-1 20:29:43

{:smile:} 很不错的

chengying 发表于 2012-11-9 10:38:21

mark

rovershie 发表于 2012-12-12 11:40:18

robotkid 发表于 2012-2-4 09:34 static/image/common/back.gif
有含量，楼主不如写流程，我来转成STM32。我也有一样的传感硬件

STM32的我写出来啦,正在整理代码上传阿.

flotox 发表于 2013-4-8 22:32:17

MARK 正在调试ITG3205

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

arduino学习笔记28 - ITG3200 ADXL345做姿态识别实验 四轴利器(一)

arduino学习笔记28 - ITG3200 ADXL345做姿态识别实验四轴利器(一)