Added some debugging messages and tweaks, switched to using Xbee as main IO