Added ServoTest code
Added a stop to MotorsTest

Preventing instantaneous reverse. Not tested yet.