Servo control working

Setting absolute or relative position (position value and velocity),
position read out and position reset is now working.
Woei also fixed invalid bit conversion that ended up in an overflow
exception.