added code for lab 3 nacigation so that it drives to designated points
from today trying to make sensor thread work