servo code should work. unclean code