Completed wall following logic