Update test2.py

Printing sonar readings so I can compare accuracy levels with an easily relatable reference. 

Moved the position of the starting position definition and changed the reversing rules. Those need more work actually.