Added more commands
Fixed bugs
Added radius determining method, fixed issues causing NPEs on the range finder

Commands are completely bugged
Merge pull request #6 from Pheenixm/master

Added more commands