3 servos working code, still need to clean up/refactor