Header File:

Main Program:

Function calculateDistance():

Function calculateOrientation():

Function checkObstacle():

Function savePosition():