Added a path calculator and changed the movement command to use the calculated path
Merge branch 'devel-B' of https://github.com/eloipuertas/ES2014B into devel-B