Changes to detection

First off we rename the file to a .cpp so we can use the Serial
library to debug it. Also s0 is always on the left and s2 is alway
on the right so we don't need to calculate their facing every time.
And lastly we added some debuffing code. We will remove this when
we have completed our testing.
Added some debugging code

When going through the nav_drive_to_target() we added some code
that will tell us what is going on.
Changed the main file

We will now run the nav_explore() and the nav_drive_to_target()
as part of the loop.
Fixed spacing issue

So glad I found this bug.