Slowpath ported. Untested: very unlikely to work. This will probably have all kinds of typos and logic errors from the transition from python to C. It's not optimised in any meaningful way either. I'm not sure if Iexpect an AVR to have the grunt necessary for this kind of floating point work every 10mm of move at any kind of speed.