fixed stucked output, added motor support

bluetooth controll finally working as supposed