improved safeties

added a servo safety limits method and checked for a glitch if w[1]-w[2]
has to go in the next quadrant.