Updated for I2C sensor switching

Merged code from old I2C program, allows for simple comment/uncomment
to activate I2C