Added code for servo arduino and test programs for I2C. I2C not quite working yet.