백래시 보정 코드

0_BACKLASH_COMPENSATION_TODO.md

• M425 코드가 있을때 자동으로 반영되게?
• G1 에서 E(압출길이)에 대한 보정도 필요한가? -> 필요시 백래시 이동코드를 추가하는 식으로 가야함.
• G28 코드는 0,0 로 이동하는 것으로 처리
  • 1,1로 갔다가 0,0으로 가야 백래시에 대한 초기화가 확실할거 같다.
• arc등은 polygon으로 변경하고, 해상도 옵션을 추가하는 것으로. (압출거리도 재계산)

backlash.py

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")

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