Added radius determining method, fixed issues causing NPEs on the range finder

Commands are completely bugged