Added M112
Added estop handling
Test and refactor
Merge pull request #851 from DanNixon/estop

Implemented M112