Added path replanning and made small fixes to lab4.py