inverse kinematics

Dependencies:   QEI mbed