Cleanup of slow and unused functions
Fixed bug where Robot would crash if an object is detected in front of it