Final

Test code for reading sonar. Problem with iterating to calculate
distance. Our code did not make it to this step.