Added obstacles to logger

Log obstacle positions. Implemented basic sort. Needs a different
algorithm for sorting, then just need to make sure next index of
obstacles[] is at least X distance, if not make it so.