just need to finish the servo commands
most of the way there