Got the Commander code to work with arbotixfirmata and the debug serial. I did it by only creating 1 software serial and having xbee use recieve portion and debugserial use transmit.