2dof_servomotor_program firstcommit

Dependencies:   mbed QEI ros_lib_kinetic USBDevice