See the description here: https://github.com/MandyMeindersma/Robotics/blob/master/Demos/Demo1/Demo1-W18-description.pdf
Code for part 2: https://github.com/MandyMeindersma/Robotics/blob/master/Demos/Demo1/wanderSM.py