Improved collision detection & segregation of debug code

Robot waits for 3sec before backing up after collision. Allows for,
say, a leaf to blow by and not be interpreted as a permanent obstruction

Debugging functions have been moved to the end of the code.