Refactored code for ease of testing.

Signed-off-by: Anton Landberg <git@atnon.se>
Unit tests for MotorControl.

Now testing MotorControl, not everything can be tested as of now,
but something is better than nothing.

Signed-off-by: Anton Landberg <git@atnon.se>