Fixed I2C and Servo code (tested on my RasPi with a wiimote)