Finished the basic robot system may still be some bugs but seems to be working well.