M112 now works

Linked emergency stop routine to M112