Added Handling of Motor Setup

Minor bugfixes as well