L3G4200D是意法(ST)半導體公司推出的一款MEMS運動傳感器:三軸數字輸出陀螺儀。可選-250~250、-500~500、-2000-2000dps
開發環境:
系統:XP
單板:Arduino Leonardo
平臺:arduino-1.0.1
目標:讀三軸陀螺儀的原始數據,並通過串口顯示
一、硬件介紹
三軸陀螺儀L3G4200D模塊的原理圖如下:
這裏只用到SCL、SDA、VCC_3.3V、GND分別連接到Arduino的對應接口上。Arduino Leonardo上直接標有SDA、SCL連上即可,其它Arduino根據自己的板子連接。
二、編寫測試代碼
現在的arduino版本高,在網上找的例程都編譯通不過,換了個低版本才編譯通過。可以參考這個上面的代碼 https://github.com/pololu/L3G4200D/tree/66f1448d7f6767e12d0fe0c5c50d4e037aedc27c/L3G4200D 找到這兩個文件L3G4200D.cpp L3G4200D.h,但文件好像不能直接下,代碼都貼在網頁上,直接copy下來。然後還要在arduino-1.0.1-windows\arduino-1.0.1\libraries下新建L3G4200D目錄,將L3G4200D.cpp L3G4200D.h拷到剛建的L3G4200D,就可以在Android中使用L3G4200D類。
文件L3G4200D.cpp
#include <L3G4200D.h>
#include <Wire.h>
#include <math.h>
// Defines ////////////////////////////////////////////////////////////////
// The Arduino two-wire interface uses a 7-bit number for the address,
// and sets the last bit correctly based on reads and writes
#define GYR_ADDRESS (0xD2 >> 1)
// Public Methods //////////////////////////////////////////////////////////////
// Turns on the L3G4200D's gyro and places it in normal mode.
void L3G4200D::enableDefault(void)
{
// 0x0F = 0b00001111
// Normal power mode, all axes enabled
writeReg(L3G4200D_CTRL_REG1, 0x0F);
}
// Writes a gyro register
void L3G4200D::writeReg(byte reg, byte value)
{
Wire.beginTransmission(GYR_ADDRESS);
Wire.write(reg);
Wire.write(value);
Wire.endTransmission();
}
// Reads a gyro register
byte L3G4200D::readReg(byte reg)
{
byte value;
Wire.beginTransmission(GYR_ADDRESS);
Wire.write(reg);
Wire.endTransmission();
Wire.requestFrom(GYR_ADDRESS, 1);
value = Wire.read();
Wire.endTransmission();
return value;
}
// Reads the 3 gyro channels and stores them in vector g
void L3G4200D::read()
{
Wire.beginTransmission(GYR_ADDRESS);
// assert the MSB of the address to get the gyro
// to do slave-transmit subaddress updating.
Wire.write(L3G4200D_OUT_X_L | (1 << 7));
Wire.endTransmission();
Wire.requestFrom(GYR_ADDRESS, 6);
while (Wire.available() < 6);
uint8_t xla = Wire.read();
uint8_t xha = Wire.read();
uint8_t yla = Wire.read();
uint8_t yha = Wire.read();
uint8_t zla = Wire.read();
uint8_t zha = Wire.read();
g.x = xha << 8 | xla;
g.y = yha << 8 | yla;
g.z = zha << 8 | zla;
}
void L3G4200D::vector_cross(const vector *a,const vector *b, vector *out)
{
out->x = a->y*b->z - a->z*b->y;
out->y = a->z*b->x - a->x*b->z;
out->z = a->x*b->y - a->y*b->x;
}
float L3G4200D::vector_dot(const vector *a,const vector *b)
{
return a->x*b->x+a->y*b->y+a->z*b->z;
}
void L3G4200D::vector_normalize(vector *a)
{
float mag = sqrt(vector_dot(a,a));
a->x /= mag;
a->y /= mag;
a->z /= mag;
}
文件L3G4200D.h:
#ifndef L3G4200D_h
#define L3G4200D_h
#include <Arduino.h> // for byte data type
// register addresses
#define L3G4200D_WHO_AM_I 0x0F
#define L3G4200D_CTRL_REG1 0x20
#define L3G4200D_CTRL_REG2 0x21
#define L3G4200D_CTRL_REG3 0x22
#define L3G4200D_CTRL_REG4 0x23
#define L3G4200D_CTRL_REG5 0x24
#define L3G4200D_REFERENCE 0x25
#define L3G4200D_OUT_TEMP 0x26
#define L3G4200D_STATUS_REG 0x27
#define L3G4200D_OUT_X_L 0x28
#define L3G4200D_OUT_X_H 0x29
#define L3G4200D_OUT_Y_L 0x2A
#define L3G4200D_OUT_Y_H 0x2B
#define L3G4200D_OUT_Z_L 0x2C
#define L3G4200D_OUT_Z_H 0x2D
#define L3G4200D_FIFO_CTRL_REG 0x2E
#define L3G4200D_FIFO_SRC_REG 0x2F
#define L3G4200D_INT1_CFG 0x30
#define L3G4200D_INT1_SRC 0x31
#define L3G4200D_INT1_THS_XH 0x32
#define L3G4200D_INT1_THS_XL 0x33
#define L3G4200D_INT1_THS_YH 0x34
#define L3G4200D_INT1_THS_YL 0x35
#define L3G4200D_INT1_THS_ZH 0x36
#define L3G4200D_INT1_THS_ZL 0x37
#define L3G4200D_INT1_DURATION 0x38
class L3G4200D
{
public:
typedef struct vector
{
float x, y, z;
} vector;
vector g; // gyro angular velocity readings
void enableDefault(void);
void writeReg(byte reg, byte value);
byte readReg(byte reg);
void read(void);
// vector functions
static void vector_cross(const vector *a, const vector *b, vector *out);
static float vector_dot(const vector *a,const vector *b);
static void vector_normalize(vector *a);
};
#endif
文件L3G4200D.ino
#include <Wire.h>
#include <L3G4200D.h>
L3G4200D gyro;
void setup() {
Serial.begin(9600);
Wire.begin();
gyro.enableDefault();
}
void loop() {
gyro.read();
Serial.print("G ");
Serial.print("X: ");
Serial.print((int)gyro.g.x);
Serial.print(" Y: ");
Serial.print((int)gyro.g.y);
Serial.print(" Z: ");
Serial.println((int)gyro.g.z);
delay(100);
}
三、編譯測試
Arduino還是很方便操作的,選擇好單板、參考,直接點上面的“對勾”就開始編譯,編譯沒問題,點“->”箭頭狀的,開始上傳程序,直至上傳進度條完成。
接着打開Tools/Serial Monitor 顯示如下:
這是水平放置的結果,傾斜模塊會看到值變化。