Motor control

Push functionality from demo to master.