Robot kinematics model fully working

Dependencies:   mbed