Built some tests for the sensors and made some minor adjustments to my implementations sonar to try and make it detect correctly