Updated with working code... still haven't verified on real servos