Removed debug print.
Changed E-stop logic, made Stop command clear E-stop fault.