Skip to content

Instantly share code, notes, and snippets.

@sng2c
Last active June 3, 2020 07:47
Show Gist options
  • Save sng2c/ab927b7447c63f7eb744ebf71ca1df81 to your computer and use it in GitHub Desktop.
Save sng2c/ab927b7447c63f7eb744ebf71ca1df81 to your computer and use it in GitHub Desktop.
백래시 보정 코드
  • M425 코드가 있을때 자동으로 반영되게?
  • G1 에서 E(압출길이)에 대한 보정도 필요한가? -> 필요시 백래시 이동코드를 추가하는 식으로 가야함.
  • G28 코드는 0,0 로 이동하는 것으로 처리
    • 1,1로 갔다가 0,0으로 가야 백래시에 대한 초기화가 확실할거 같다.
  • arc등은 polygon으로 변경하고, 해상도 옵션을 추가하는 것으로. (압출거리도 재계산)
import logging
class Gcode:
@classmethod
def parse(cls, lines):
for line in lines:
yield cls.fromStr(line)
@classmethod
def _tokenize_gcode(cls, line):
chunks = line.split(';', 1)
comment = ''
if len(chunks) == 2:
comment = ';'+chunks[1].rstrip()
if len(chunks[0]) == 0:
return None,None
return chunks[0].rstrip().split(),comment
@classmethod
def _parse_param(cls, token):
return token[0], float(token[1:])
@classmethod
def fromStr(cls, cmdstr):
cmdstr = cmdstr.strip()
tokens,comment = cls._tokenize_gcode(cmdstr)
cmd = tokens[0] if tokens is not None else None
if cmd in ['G0', 'G1', 'G28', 'M425']:
params = dict(cls._parse_param(token) for token in tokens[1:]) if tokens is not None else None
else:
cmd = None
params = None
return cls(cmd, params, comment, cmdstr)
def __init__(self, cmd, params, comment='', rawdata=''):
self.cmd = cmd
self.params = params
self.rawdata = rawdata
self.comment = comment
def __repr__(self):
if self.cmd is not None:
return "Gcode('{}', '{}', '{}', '{}')".format(self.cmd, self.params, self.comment, self.rawdata)
else:
return self.rawdata
def __str__(self):
return ' '.join([self.cmd, *[k + str(round(self.params[k],4)).rstrip('0').rstrip('.') for k in
self.params], self.comment]) if self.cmd is not None else self.rawdata
class Axis:
def __init__(self, lash=0.0, correction=1.0, offset=0.0, pos=0.0, direction=0, err=0.0):
self.pos = float(pos)
self.lash = float(lash)
self.error = float(err)
self.correction = float(correction)
self.direction = direction
self.offset = float(offset)
def _new_direction(self, newpos):
delta = newpos - self.pos
if delta > 0:
newdir = 1
elif delta < 0:
newdir = -1
else:
newdir = self.direction
return newdir
def reset(self):
self.direction = 0
self.pos = 0
def move_to(self, newpos):
last_calc_pos = self.calc_pos()
newdir = self._new_direction(newpos)
preceding_pos = None
if newdir != self.direction:
self.direction = newdir
preceding_err = self.calc_err() * self.direction
if preceding_err != 0:
preceding_pos = last_calc_pos + preceding_err
self.pos = newpos
return preceding_pos
def calc_err(self):
return self.lash * self.correction
def calc_pos(self):
# when forward direction, append error
err_factor = 1 if self.direction > 0 else 0
return self.pos + self.offset + (self.calc_err()*err_factor)
def __repr__(self):
return "Axis(lash={}, correction={}, offset={}, pos={}, direction={})".format(self.lash, self.correction, self.offset, self.pos, self.direction)
def __str__(self):
return self.__repr__()
if __name__ == '__main__':
logging.basicConfig(level=logging.DEBUG)
import argparse
parser = argparse.ArgumentParser(description='Backlash Compensator', usage='%(prog)s -x 0.6 -y 0.6 sample.gcode -o out.gcode')
parser.add_argument('-x','--x-dist', help='X_DISTANCE_MM', type=float, default=0.0)
parser.add_argument('-X','--x-offset', help='X_OFFSET_MM', type=float, default=0.0)
parser.add_argument('-y','--y-dist', help='Y_DISTANCE_MM', type=float, default=0.0)
parser.add_argument('-Y','--y-offset', help='Y_OFFSET_MM', type=float, default=0.0)
parser.add_argument('-z','--z-dist', help='Z_DISTANCE_MM', type=float, default=0.0)
parser.add_argument('-Z','--z-offset', help='Z_OFFSET_MM', type=float, default=0.0)
parser.add_argument('-c','--correction', help='CORRECTION', type=float, default=1.0)
parser.add_argument('input', help='INPUT G-code', type=str)
parser.add_argument('-o', '--output', type=str, default='out.gcode')
args = parser.parse_args()
INPUT = args.input
OUTPUT = args.output
CORRECTION = args.correction
X_DISTANCE_MM = args.x_dist
Y_DISTANCE_MM = args.y_dist
Z_DISTANCE_MM = args.z_dist
X_OFFSET = args.x_offset
Y_OFFSET = args.y_offset
Z_OFFSET = args.z_offset
axes = {
'X': Axis(lash=X_DISTANCE_MM, offset=X_OFFSET, correction=CORRECTION),
'Y': Axis(lash=Y_DISTANCE_MM, offset=Y_OFFSET, correction=CORRECTION),
'Z': Axis(lash=Z_DISTANCE_MM, offset=Z_OFFSET, correction=CORRECTION),
}
def print_axes(axes):
for sign in axes:
print("{}: {}".format(sign, axes[sign]))
print_axes(axes)
with open(INPUT) as gcode_data:
with open(OUTPUT,'w') as output:
for gcode in Gcode.parse(gcode_data):
if gcode.cmd in ['G0', 'G1']:
for sign in gcode.params:
if sign in axes:
preceding_pos = axes[sign].move_to(gcode.params[sign])
if preceding_pos is not None:
gcode_c = Gcode(
'G1', {sign: preceding_pos},
';fix {}'.format( 'fwd' if axes[sign].direction == 1 else 'bwd' ))
output.write(str(gcode_c)+"\n")
gcode.params[sign] = axes[sign].calc_pos()
if gcode.cmd in ['G28']:
axes['X'].reset()
axes['Y'].reset()
if gcode.cmd in ['M425']:
if 'F' in gcode.params:
for sign in axes:
axes[sign].correction = gcode.params['F']
for sign in axes:
if sign in gcode.params:
axes[sign].lash = gcode.params[sign]
print("Applied ",gcode)
print_axes(axes)
# make comment M425
gcode.cmd = None
gcode.rawdata = ';Backlash Compensation by '+gcode.rawdata
output.write(str(gcode)+"\n")
@sng2c
Copy link
Author

sng2c commented Jun 3, 2020

M425코드에 반응하게 해놨습니다.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment