Skip to content

Commit ae7a37a

Browse files
randymxjrandymxj
committed
Initial upload for the MPU6050 Triple Axis Accelerometer and Gyro sensor
1 parent 9e81e41 commit ae7a37a

File tree

4 files changed

+178
-0
lines changed

4 files changed

+178
-0
lines changed

‎Adafruit_HTU21D/Adafruit_HTU21D.py

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,9 @@
55

66
# ===========================================================================
77
# HTU21D Class
8+
#
9+
# Code only test with a Sparkfun HTU21D Sensor module on a Beaglebone Black.
10+
# It has been reported that an I2C address issue was seen on a Pi.
811
# ===========================================================================
912

1013
class HTU21D:

‎Adafruit_MPU6050/Adafruit_I2C.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
../Adafruit_I2C/Adafruit_I2C.py

‎Adafruit_MPU6050/Adafruit_MPU6050.py

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
#!/usr/bin/python
2+
3+
import time, datetime, math
4+
from Adafruit_I2C import Adafruit_I2C
5+
6+
# ===========================================================================
7+
# MPU6050 Class
8+
#
9+
# Ported from http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html
10+
# ===========================================================================
11+
12+
class MPU6050:
13+
i2c = None
14+
15+
# MPU6050 Address
16+
address = 0x68
17+
18+
# Command / Register Address
19+
MPU6050_PWR_MGMT_1 = 0x6B #R/W
20+
MPU6050_PWR_MGMT_2 = 0x6C #R/W
21+
22+
MPU6050_RA_ACCEL_XOUT_H = 0x3B #R
23+
MPU6050_RA_ACCEL_XOUT_L = 0x3C #R
24+
MPU6050_RA_ACCEL_YOUT_H = 0x3D #R
25+
MPU6050_RA_ACCEL_YOUT_L = 0x3E #R
26+
MPU6050_RA_ACCEL_ZOUT_H = 0x3F #R
27+
MPU6050_RA_ACCEL_ZOUT_L = 0x40 #R
28+
MPU6050_RA_TEMP_OUT_H = 0x41 #R
29+
MPU6050_RA_TEMP_OUT_L = 0x42 #R
30+
MPU6050_RA_GYRO_XOUT_H = 0x43 #R
31+
MPU6050_RA_GYRO_XOUT_L = 0x44 #R
32+
MPU6050_RA_GYRO_YOUT_H = 0x45 #R
33+
MPU6050_RA_GYRO_YOUT_L = 0x46 #R
34+
MPU6050_RA_GYRO_ZOUT_H = 0x47 #R
35+
MPU6050_RA_GYRO_ZOUT_L = 0x48 #R
36+
37+
MPU6050_PWR1_SLEEP_BIT = 6
38+
39+
# Constant
40+
pi = 3.1415926
41+
AcceRatio = 16384.0
42+
GyroRatio = 131.0
43+
44+
# Variable
45+
offset_acc_x = 0
46+
offset_acc_y = 0
47+
offset_acc_z = 0
48+
offset_gyr_x = 0
49+
offset_gyr_y = 0
50+
offset_gyr_z = 0
51+
52+
last_read_time = datetime.datetime.now()
53+
54+
# Constructor
55+
def __init__(self):
56+
self.i2c = Adafruit_I2C(self.address)
57+
self.initialize()
58+
59+
def initialize(self):
60+
"Initiate the sensor by set and clear the power sleep bit, and collector an average offset"
61+
62+
# Set the sleep bit
63+
self.setBit(self.MPU6050_PWR_MGMT_1, self.MPU6050_PWR1_SLEEP_BIT)
64+
# Clear the sleep bit
65+
self.clearBit(self.MPU6050_PWR_MGMT_1, self.MPU6050_PWR1_SLEEP_BIT)
66+
# The above Set/Clear process is necessary to prevent the sensor going halt or freeze
67+
68+
# Take simple and calculate the average offset
69+
simple_time = 200
70+
sum_acc_x = 0
71+
sum_acc_y = 0
72+
sum_acc_z = 0
73+
sum_gyro_x = 0
74+
sum_gyro_y = 0
75+
sum_gyro_z = 0
76+
for i in range(simple_time):
77+
x_accel = self.readSint16(self.MPU6050_RA_ACCEL_XOUT_H) / self.AcceRatio
78+
y_accel = self.readSint16(self.MPU6050_RA_ACCEL_YOUT_H) / self.AcceRatio
79+
z_accel = self.readSint16(self.MPU6050_RA_ACCEL_ZOUT_H) / self.AcceRatio
80+
81+
x_gyro = self.readSint16(self.MPU6050_RA_GYRO_XOUT_H) / self.AcceRatio
82+
y_gyro = self.readSint16(self.MPU6050_RA_GYRO_YOUT_H) / self.AcceRatio
83+
z_gyro = self.readSint16(self.MPU6050_RA_GYRO_ZOUT_H) / self.AcceRatio
84+
85+
temperature = self.readSint16(self.MPU6050_RA_TEMP_OUT_H)
86+
temperature = (temperature + 521.0) / 340.0 + 35.0
87+
88+
sum_acc_x += x_accel
89+
sum_acc_y += y_accel
90+
sum_acc_z += z_accel
91+
92+
sum_gyro_x += x_gyro
93+
sum_gyro_y += y_gyro
94+
sum_gyro_z += z_gyro
95+
96+
self.offset_acc_x = sum_acc_x / simple_time
97+
self.offset_acc_y = sum_acc_y / simple_time
98+
self.offset_acc_z = sum_acc_z / simple_time
99+
100+
self.offset_gyr_x = sum_gyro_x / simple_time
101+
self.offset_gyr_y = sum_gyro_y / simple_time
102+
self.offset_gyr_z = sum_gyro_z / simple_time
103+
104+
def setBit(self, register, bit):
105+
"Set a bit in the register"
106+
# Read
107+
original = self.i2c.readU8(register)
108+
109+
# Set
110+
self.i2c.write8( register, original | ( 0x01 << bit ) )
111+
112+
def clearBit(self, register, bit):
113+
"Clear a bit in the register"
114+
# Read
115+
original = self.i2c.readU8(register)
116+
117+
# Clear
118+
self.i2c.write8( register, original ^ ( 0x01 << bit ) )
119+
120+
def readSint16(self, address):
121+
"Read a signed 16 bits register"
122+
high = self.i2c.readU8(address)
123+
low = self.i2c.readU8(address + 1)
124+
value = (high << 8) + low
125+
126+
if (value >= 0x8000):
127+
return -((65535 - value) + 1)
128+
else:
129+
return value
130+
131+
def readMPU6050(self):
132+
"Read and return the Gyro and the Accelerometer value"
133+
134+
current_time = datetime.datetime.now()
135+
dt_timedelta = ( current_time - self.last_read_time )
136+
dt_milliseconds = ( dt_timedelta.seconds * 1000 * 1000 + dt_timedelta.microseconds ) / 1000
137+
self.last_read_time = current_time
138+
139+
x_accel = self.readSint16(self.MPU6050_RA_ACCEL_XOUT_H) / self.AcceRatio
140+
y_accel = self.readSint16(self.MPU6050_RA_ACCEL_YOUT_H) / self.AcceRatio
141+
z_accel = self.readSint16(self.MPU6050_RA_ACCEL_ZOUT_H) / self.AcceRatio
142+
143+
x_gyro = self.readSint16(self.MPU6050_RA_GYRO_XOUT_H) / self.AcceRatio
144+
y_gyro = self.readSint16(self.MPU6050_RA_GYRO_YOUT_H) / self.AcceRatio
145+
z_gyro = self.readSint16(self.MPU6050_RA_GYRO_ZOUT_H) / self.AcceRatio
146+
147+
temperature = self.readSint16(self.MPU6050_RA_TEMP_OUT_H)
148+
temperature = (temperature + 521.0) / 340.0 + 35.0
149+
150+
x_rotation = -math.degrees( math.atan2(x_accel, math.sqrt((y_accel*y_accel)+(z_accel*z_accel))) )
151+
y_rotation = math.degrees( math.atan2(y_accel, math.sqrt((x_accel*x_accel)+(z_accel*z_accel))) )
152+
153+
return x_accel, y_accel, z_accel, x_gyro, y_gyro, z_gyro, temperature, x_rotation, y_rotation
154+

‎Adafruit_MPU6050/Adafruit_MPU6050_example.py

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
#!/usr/bin/python
2+
3+
import time
4+
from Adafruit_MPU6050 import MPU6050
5+
6+
# ===========================================================================
7+
# Example Code
8+
# ===========================================================================
9+
10+
# Initialise the MPU6050
11+
mpu = MPU6050()
12+
13+
# read
14+
for _ in range(1000):
15+
16+
x_accel, y_accel, z_accel, x_gyro, y_gyro, z_gyro, temperature, x_rotation, y_rotation = mpu.readMPU6050()
17+
18+
print "Accel X: %.2f, Y: %.2f, Z: %.2f, Gyro X: %.2f, Y: %.2f, Z: %.2f, Temperature: %.2f, x_rotation: %.2f, y_rotation: %.2f" % (x_accel, y_accel, z_accel, x_gyro, y_gyro, z_gyro, temperature, x_rotation, y_rotation)
19+
20+
time.sleep(0.1)

0 commit comments

Comments
 (0)