Added Function for emergency stop #12, see TODOs in code