import mbed
import sys
def float(n):
s = 1.0
if n < 0:
s = -1.0
n *= -1
f = 0.0
a = 1.0
while n:
if n & 1:
f += a
n >>= 1
a *= 2.0
f *= s
return f
def abs(a):
if a < 0:
return -a
return a
class MMA8451Q:
REG_WHO_AM_I=0x0D
REG_CTRL_REG_1=0x2A
REG_OUT_X_MSB=0x01
REG_OUT_Y_MSB=0x03
REG_OUT_Z_MSB=0x05
UINT14_MAX=16383
def __init__(self, sda, scl, addr):
self._i2c = mbed.I2C(sda, scl)
self._addr = addr
data = chr(self.REG_CTRL_REG_1)+chr(0x01)
self.writeRegs(data)
def getAccX(self):
return float(self.getAccAxis(self.REG_OUT_X_MSB)) / 4096.0
def getAccY(self):
return float(self.getAccAxis(self.REG_OUT_Y_MSB)) / 4096.0
def getAccZ(self):
return float(self.getAccAxis(self.REG_OUT_Z_MSB)) / 4096.0
def getWhoAmI(self):
who_am_i = chr(0)
self.readRegs(self.REG_WHO_AM_I, who_am_i);
return ord(who_am_i)
def getAccAxis(self, addr):
res = chr(0)+chr(0)
self.readRegs(addr, res)
acc = (ord(res[0]) << 6) | (ord(res[1]) >> 2)
if acc > (self.UINT14_MAX/2):
acc -= self.UINT14_MAX;
return acc
def readRegs(self, addr, data):
self._i2c.write(self._addr, chr(addr), 1, 1)
self._i2c.read(self._addr, data, len(data), 0)
def writeRegs(self, data):
self._i2c.write(self._addr, data, len(data), 0)
MMA8451_I2C_ADDRESS=(0x1d<<1)
acc = MMA8451Q('PTE25', 'PTE24', MMA8451_I2C_ADDRESS)
rled = mbed.PwmOut('LED_RED')
gled = mbed.PwmOut('LED_GREEN')
bled = mbed.PwmOut('LED_BLUE');
while 1:
rled.write(1.0 - abs(acc.getAccX()))
gled.write(1.0 - abs(acc.getAccY()))
bled.write(1.0 - abs(acc.getAccZ()))
sys.wait(100)