Updated MotionController

Implemented support for endstop checking, and got some preliminary code
in place for "G1 X_ Y_ Z_". In theory it should work... but it's
untested so far.