Testing servo code