I2C Servo Motor Firmware