Added support for stopping i2c as well
Fixed support for CSMD board

Slightly tricker as I2C shares some pins with the servo connections