Adding collision avoidance code still buggy