Added function to calculate motor speeds from given angle, velocity and
rotation speed. (Yet only simulated, untested on real robot).