Add a testing mode

[blhough]

We will want a way to easily test motors, sensors, etc individually and together