Motor selftest
Removing some debugging stuff