Du must diese Zeile aktivieren:
// case 1: das da aktivieren für 2 mS Servo

sollte er in die andere Ecke