M200 implementation
Fixed math

This is why I wanted to sleep on the code I wrote while falling asleep
rather than immediately submitting a pull request.
Merge pull request #737 from whosawhatsis/M200

M200 implementation
Merge branch 'Marlin_v1' into mendel90