Add M05 code, fix G0 code