Implementation of Schunk Motion Protocol in C++