Merge pull request #49 from mlosch/master

Goto controller running
Merge branch 'master' of https://github.com/KTH-RAS-HT14-G9/controllers
Merge remote-tracking branch 'max/master'
fix some bugs in goto