Finished I2C Write Function