Added better manual movement code

Untested because i don't have access to the hardware right now,
but merged from my working Codebase, only removed EndStop voodoo that will follow in another pull-request,
so it should work.

At least it does compile!