G0 and G1 implemented for Gcode, need testing