This simplifies the wr-servo state machine by factorizing out some
things, removing some conditionals and using the state strings array
instead of several strcpy.

As a side effect, it requires less iterations when the offset is
less than one second, and it reports more correctly the current status
to diagnostics and wr_mon.

Signed-off-by: Alessandro Rubini <rubini@gnudd.com>