-
Notifications
You must be signed in to change notification settings - Fork 7
/
gyro.py
56 lines (44 loc) · 1.62 KB
/
gyro.py
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
#!/usr/bin/python
from time import sleep
#import smbus to access i2c port
import smbus
import string
#converts 16 bit two's compliment reading to signed int
def getSignedNumber(number):
if number & (1 << 15):
return number | ~65535
else:
return number & 65535
#open /dev/i2c-1
i2c_bus=smbus.SMBus(1)
#i2c slave address of the L3G4200D
i2c_address=0x69
#initialise the L3G4200D
#normal mode and all axes on to control reg1
i2c_bus.write_byte_data(i2c_address,0x20,0x0F)
#full 2000dps to control reg4
i2c_bus.write_byte_data(i2c_address,0x23,0x20)
#read lower and upper bytes, combine and display
while True:
i2c_bus.write_byte(i2c_address,0x28)
X_L = i2c_bus.read_byte(i2c_address)
i2c_bus.write_byte(i2c_address,0x29)
X_H = i2c_bus.read_byte(i2c_address)
X = X_H << 8 | X_L
i2c_bus.write_byte(i2c_address,0x2A)
Y_L = i2c_bus.read_byte(i2c_address)
i2c_bus.write_byte(i2c_address,0x2B)
Y_H = i2c_bus.read_byte(i2c_address)
Y = Y_H << 8 | Y_L
i2c_bus.write_byte(i2c_address,0x2C)
Z_L = i2c_bus.read_byte(i2c_address)
i2c_bus.write_byte(i2c_address,0x2D)
Z_H = i2c_bus.read_byte(i2c_address)
Z = Z_H << 8 | Z_L
X = getSignedNumber(X)
Y = getSignedNumber(Y)
Z = getSignedNumber(Z)
print string.rjust(`X`, 10),
print string.rjust(`Y`, 10),
print string.rjust(`Z`, 10)
sleep(0.02)