Movement getting jerky if M2120 is used

[Felix-fh]

While using the position update with command M2120 the movement of the uArm gets uneven and interrupted.

[mattuna15]

Hi @Felix-fh i have created a new branch (#9) to test some new maths routines. I wonder if you could see if it makes a difference to this? Would be interested to know if it is a useful direction to go in or if it doesn't help at all.