Tested system on a 32x32 map, minor bugfixes, but works great.

Start position currently in both Map and Controller. New start position set to (5,10)