Not fully optimized Gcode but good enough !
gcode seems to work, need testing on real machine
Merge branch 'gcode'