My code is not fast enough, i don't know why

For real time you need to accomplish this in hal with hal components, not gcode.
You can arm/enable the hal logic with M66, but the actual real time logic must be
implemented in hal.