test and possible bug fixes
test sensors
test code
more tests
distance formula
removed old stuff