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)