Basic testing of code with Xbee. Runs through without errors. Not tested for actual motion control!