def set_speed(bus, motor, speed):
if (speed < 0):
speed = ((abs(speed) ^ 0xffff) + 1) & 0xffff
i2c.writel(bus, motor, 0x04, 0x0000)
i2c.writel(bus, motor, 0x06, speed)
i2c.writel(bus, motor, 0x90, 0x0004)
i2c.writel(bus, motor, 0x80, 0xffff)
i2c.write(bus, motor, 0x08, 0x00)
def init_gyro(bus, gyro):
"""
gyro scale register values
FS_SEL | Full Scale Range | LSB Sensitivity
-------+--------------------+----------------
0 | +/- 250 degrees/s | 131 LSB/deg/s
1 | +/- 500 degrees/s | 65.5 LSB/deg/s
2 | +/- 1000 degrees/s | 32.8 LSB/deg/s
3 | +/- 2000 degrees/s | 16.4 LSB/deg/s
accel scale register values
AFS_SEL | Full Scale Range | LSB Sensitivity
--------+------------------+----------------
0 | +/- 2g | 8192 LSB/mg
1 | +/- 4g | 4096 LSB/mg
2 | +/- 8g | 2048 LSB/mg
3 | +/- 16g | 1024 LSB/mg
"""
i2c.write(bus, gyro, 0x6b, 0x00) # activate sensor (asleep by default)
i2c.write(bus, gyro, 0x1b, 0x00) # gyro scale register
i2c.write(bus, gyro, 0x1c, 0x00) # accel scale register
def init_motor(bus, motor):
i2c.write(bus, motor, 0x01, 0x13)
