Updated delay for test function.
Fixed bug with negative speed.