Added initial threading
Added home sub routine to reset servo positions
Tidied code a little
Added inital code for speed options