Witam, czy ktoś stosował gwintowania na sztywno z wykorzystaniem enkodera wrzeciona?

W mojej maszynce zastosowałem wrzeciono napędzane serwonapędem. Wykorzystuję sygnał zwrotny z enkodera do pomiaru prędkości obrotowej na wejściach 15,16,17 w Pikocnc. Zgodnie z instrukcją zastosowałem G33 k X Y Z. Problem polega na tym że w kodzie nie ma od razu za implikowanego wyjazdu gwintownika z nagwintowanego otworu. Kod też nie zawiera komendy zatrzymania wrzeciona na dnie otworu. Zatrzymując wrzeciono M5 niestety nie sprawdza się ponieważ piko nie synchronizuje się już podczas tej komendy i następuje zniszczenie gwintu. Jeżeli ktoś zetknął się już z takim problemem i udało się go rozwiązać to proszę o pomoc. Jak powinien wyglądać prawidłowy G kod?