Fix bugs/change certain things in dynamixel library
Write sample program to control robot