Tried to fix gcode generation, still some problems