Jak napisać procedurę sterowania głowica obrotową.

Witajcie.

Mam głowicę obrotową, Posiada czujnik pierwszego narzędzia, który można traktować jako bazę. Ponadto każde przerzucenie do następnej pozycji jest kwitowane sygnałem z drugiego czujnika. Pytanie jak napisać procedurę tak aby jednym G-kodem zawsze prawidłowo wybrać narzędzie? Oczywiście przy pierwszej zmianie musi być najechanie na bazę jeśli nie jest akurat wybrane narzędzie numer jeden. Czyli chodzi o to aby pamiętać ostatnio wybrane narzędzie i wygenerować odpowiednią ilość impulsów aby przerzucić głowicę na pożądaną pozycję. Oczywiście po każdym impulsie trzeba poczekać na potwierdzenie przerzucenia.

Nie miałbym z tym problemu w Machu ale jeśli chodzi o linuxcnc to jestem zielony jak nać pietruszki.

Czy w ogóle jest możliwość aby M6 wywoływało tę procedurę czy musi to być np M106?