Hey Richard!
The axes velocities (X,Y,Z,A,B,C) have to be constant in my case and are set by the user.
Do you mean something like (using abstract syntax notation):
while(1)
{
(Xcurrent+dX):Xvel (Ycurrent+dY):Yvel (Zcurrent+dZ):Zvel (Acurrent+dA):Avel (Bcurrent+dB):Bvel (Ccurrent+dC):Cvel
if (CurrentPos==(CommandedPos-deltaPos))
{
X:0 Y:0 Z:0
break
}
}