Adding servo control to i2c