Added method to get distance between two points
Added method to get angle of the line between two points
Testing implementation of direction control
Stashing commits to merge develop branch into master