Improve Voltage Checking

I am trying to fix some issues with the voltage testing using the servo
voltage values.  I think this version should work better.  It also
checks while interpolation is active, but tries to only do so when it
thinks there is enough time for the read of the AX register without
interfering with the start of the next interpolation output.

Will also try to fold same changes into the main branch as well