Added robotis library and beginnings of test code