range finder code update
Added Test Functions for Range Finder
Fixed IR health issue.