Merge branch 'origin/reorganisation'
Changed the location of the pid function
Added driveController() functionality
Obstacle avoidance implemented. Triggered some debugging problems... take care with this commit.
pathFinder fully implemented. "Worst case" runtime: 365ms
Added functions to set the target angular velocity of the controller
Includes and comment improvement
Merge branch 'origin/develop'