Servo code reformat

Lots of changes were to made to improve performance: preventing
glutting of toggles and clarifying distinct state flips based on a
variable, using case switches instead of if else pairs.