def getAccel(self): mpu = self.mpu aX = MPU6000.smashhl(mpu.readReg(MPU6000.R_ACCEL_XOUT_H), mpu.readReg(MPU6000.R_ACCEL_XOUT_L)) aY = MPU6000.smashhl(mpu.readReg(MPU6000.R_ACCEL_YOUT_H), mpu.readReg(MPU6000.R_ACCEL_YOUT_L)) aZ = MPU6000.smashhl(mpu.readReg(MPU6000.R_ACCEL_ZOUT_H), mpu.readReg(MPU6000.R_ACCEL_ZOUT_L)) raw_accel = (aX, aY, aZ) accel = tuple([d / 16384.0 for d in raw_accel]) return accel
def getGyro(self): mpu = self.mpu gyX = MPU6000.smashhl(mpu.readReg(MPU6000.R_GYRO_XOUT_H), mpu.readReg(MPU6000.R_GYRO_XOUT_L)) gyY = MPU6000.smashhl(mpu.readReg(MPU6000.R_GYRO_YOUT_H), mpu.readReg(MPU6000.R_GYRO_YOUT_L)) gyZ = MPU6000.smashhl(mpu.readReg(MPU6000.R_GYRO_ZOUT_H), mpu.readReg(MPU6000.R_GYRO_ZOUT_L)) raw_gyro = (gyX, gyY, gyZ) gyro = tuple([d / 131.0 for d in raw_gyro]) return gyro
def pollFifo(self):
while self.polling:
count = self.getFifoCount()
while count > 0:
if(count % 12 != 0):
self.resetFifo()
break
aX = MPU6000.smashhl(self.getFifo(), self.getFifo())
aY = MPU6000.smashhl(self.getFifo(), self.getFifo())
aZ = MPU6000.smashhl(self.getFifo(), self.getFifo())
gX = MPU6000.smashhl(self.getFifo(), self.getFifo())
gY = MPU6000.smashhl(self.getFifo(), self.getFifo())
gZ = MPU6000.smashhl(self.getFifo(), self.getFifo())
raw_accel = aX,aY,aZ
raw_gyro = gX,gY,gZ
gyro = tuple([d / (131.0 * 58.82) for d in raw_gyro])
accel = tuple([d / 16384.0 for d in raw_accel])
#self.GYRO_BIAS = tuple(map(operator.div,tuple(map(operator.add, self.GYRO_BIAS, gyro)), (2,2,2)))
gyro = tuple(map(operator.add, self.CUR_ROT, gyro))
self.CUR_ROT = tuple(map(operator.add, gyro, self.GYRO_BIAS))
count = self.getFifoCount()
time.sleep(.01)
if xGyro > 32767:
xGyro -= 65536
yGyro = data[2] * 256 + data[3]
if yGyro > 32767:
yGyro -= 65536
zGyro = data[4] * 256 + data[5]
if zGyro > 32767:
zGyro -= 65536
return {'x': xGyro, 'y': yGyro, 'z': zGyro}
from MPU6000 import MPU6000
mpu6000 = MPU6000()
while True:
mpu6000.gyro_config()
mpu6000.accl_config()
mpu6000.power_management_1()
time.sleep(0.1)
accl = mpu6000.read_accl()
print "Acceleration in X-Axis : %d" % (accl['x'])
print "Acceleration in Y-Axis : %d" % (accl['y'])
print "Acceleration in Z-Axis : %d" % (accl['z'])
gyro = mpu6000.read_gyro()
print "X-Axis of Rotation : %d" % (gyro['x'])
print "Y-Axis of Rotation : %d" % (gyro['y'])
print "Z-Axis of Rotation : %d" % (gyro['z'])
print " ************************************ "
def getFifoCount(self): mpu = self.mpu fifo = MPU6000.smashhl(mpu.readRegU(MPU6000.R_FIFO_COUNT_H), mpu.readRegU(MPU6000.R_FIFO_COUNT_L)) return fifo
def getTemp(self): mpu = self.mpu temph = mpu.readReg(MPU6000.R_TEMP_OUT_H) templ = mpu.readReg(MPU6000.R_TEMP_OUT_L) temp = MPU6000.smashhl(temph, templ) return (int(temp) / 340.0) + 36.53