Code for front servo tested