first commit
Added in basica Xbee communication and cleaned up the code to make it more readable.
sonars were reading too slow. added tcp communication for writing messages, we will now be reading a single set of data and storing the previous 2