Problems "simulating" servos

For my project I need Servo-Joints. So I have subclassed RevoluteJoint and added some code lines for setting a destination angle in the interval from 0..180°. The current angle is tracked through get-joint-angle, converted to degree and rounded to integer. Within the iterate method I only test, if the destination angle is reached and set the velocity of the joint to zero.
Another call to set-servo would reactivate the joint with setting the velocity to != 0.
Now the problem: I need a quite fast servo. But setting the velocity of a joint > ~0.4 lets the joint move with steps > 1°. In other words, from simulation tick t to simulation tick t+1 a servo angle can advance from e.g.
40° to 43°. Now my test if the angle to stop is reached, (currAngle == dstangle) ist not sufficient (imaging in the example I set the servo to 41°).
Sure I could test not for a certain degree but if the currAngle is greater or lower than the desAngle. But if my dstAngle is overstepped I realize that but I am not able to correct it, because the RevoluteJoint has no set-joint-angle Method, where I could set it to dstAngle.

So my Problem: My servo-class only works for very slow joint movements, but I need much faster ones. Any Ideas?

[code]
+to iterate:
currRadAngle(double).

if direction != 0:
{
currRadAngle = (self rad-to-deg value(self get-joint-angle)) +
90.
currAngle = (self round value currRadAngle).

if currAngle == dstAngle:
{
direction = 0.
super set-joint-velocity to 0.
}

}

super iterate.
[/code]