Completed setdist code, made a few changes to test servo