1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
|
//Arduino 1.0+ only
#include <Wire.h>
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
int L3G4200D_Address = 104; //I2C address of the L3G4200D
int x;
int y;
int z;
void writeRegister(int deviceAddress, byte address, byte val) {
Wire.beginTransmission(deviceAddress); // start transmission to device
Wire.write(address); // send register address
Wire.write(val); // send value to write
Wire.endTransmission(); // end transmission
}
int readRegister(int deviceAddress, byte address){
int v;
Wire.beginTransmission(deviceAddress);
Wire.write(address); // register to read
Wire.endTransmission();
Wire.requestFrom(deviceAddress, 1); // read a byte
while(!Wire.available()) {
// waiting
}
v = Wire.read();
return v;
}
void getGyroValues(){
byte xMSB = readRegister(L3G4200D_Address, 0x29);
byte xLSB = readRegister(L3G4200D_Address, 0x28);
x = ((xMSB << 8) | xLSB);
byte yMSB = readRegister(L3G4200D_Address, 0x2B);
byte yLSB = readRegister(L3G4200D_Address, 0x2A);
y = ((yMSB << 8) | yLSB);
byte zMSB = readRegister(L3G4200D_Address, 0x2D);
byte zLSB = readRegister(L3G4200D_Address, 0x2C);
z = ((zMSB << 8) | zLSB);
}
int setupL3G4200D(int scale){
//From Jim Lindblom of Sparkfun's code
// Enable x, y, z and turn off power down:
writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);
// If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);
// Configure CTRL_REG3 to generate data ready interrupt on INT2
// No interrupts used on INT1, if you'd like to configure INT1
// or INT2 otherwise, consult the datasheet:
writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);
// CTRL_REG4 controls the full-scale range, among other things:
if(scale == 250){
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
}else if(scale == 500){
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
}else{
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
}
// CTRL_REG5 controls high-pass filtering of outputs, use it
// if you'd like:
writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}
void setup(){
Wire.begin();
Serial.begin(9600);
Serial.println("starting up L3G4200D");
setupL3G4200D(2000); // Configure L3G4200 - 250, 500 or 2000 deg/sec
delay(1500); //wait for the sensor to be ready
}
void loop(){
getGyroValues(); // This will update x, y, and z with new values
Serial.print("X:");
Serial.print(x);
Serial.print(" Y:");
Serial.print(y);
Serial.print(" Z:");
Serial.println(z);
delay(100); //Just here to slow down the serial to make it more readable
}
|