Updated XBee code

Should be functional now. Still needs to be commented thoroughly.