Emergency stop

What's new
=========

* Emergency stop added

What's next
=========

* Heavy code optimization
* Servo support