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

Need to be tested on the hand to (and thresholds probably in motor_tests.cpp needs to be changed too)