Schau mal hier, da findest Du einige Beispiele, auch in C:
http://www.acroname.com/robotics/inf...1/srf08-1.html

Vielleicht hilft es Dir