Added functions for navigating to a point. Still needs to be tested. There is still a problem in Convert.nxc -> ConvertCarthesian... with math functions
Merge branch 'master' of github.com:deaddog/sw505-code