Added motor self tests - Fixes #11 and Fixes #14