Arduino筆記五三軸陀螺儀L3G4200D

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 顯示如下：

這是水平放置的結果，傾斜模塊會看到值變化。