Switch off the servos during I2C mode, enable already implemented
Code cleanup
The timers are set during the start of the main loop