Nie wiem z czego generujesz g-kod ale ja robię tak, że rysuje element wykorzystując warstwy w jakimś programie cad (ja korzystam z Q-cad) i zapisuje jako dxf. Wczytując do macha określasz co ile ma się zagłębiać oś Z w kolejnych warstwach i po sprawie - prosto i skutecznie.