Cleaned up a little after merge, added more to robot class, added more testing in sData.py