Completed initial sketch of sonar code. Testing begins :)