NumpyでGコードをパルスの配列に変換してみた

NumpyでGコードをパルスの配列に変換してみました。

目的

Repetierで3Dプリンター用のGコードを生成すると、造形物の印刷部分は下記のようになります。

G0 X86.4 Y86.4 Z0.3 F9000
G1 X113.6 Y86.4 E1.35701 F1800
.
.
.

これを

1, 0, 0
1, 0, 0
1, 0, 0
.
.
.

といったような、XYZ軸それぞれのモータードライバに送信するパルスを0、1で列挙した配列を生成したいと思います。

処理内容

Gコードを座標の配列に変換する

PyQtGraphにRepetierで生成したGコードをプロットしてみたで作成したGcodeクラスを使って、Gコードを座標の配列に変換します。次のコードを実行して、

with open(filepath, 'r') as f:
    gcode = Gcode( f.read() )

gcode = np.delete( gcode.print_lines[1], [3, 4, 5], 1 )

次のように座標のnumpy配列が得られます。

[[ 86.4    86.4     0.3  ]
 [113.6    86.4     0.3  ]
 [113.6   113.6     0.3  ]
 ...
 [ 91.694  90.739  20.1  ]
 [ 91.128  90.739  20.1  ]
 [ 90.74   91.127  20.1  ]]

座標の差分のとる

座標のnumpy配列から各要素ごとの差分をとるために下記のコードを実行します。

gcode_diff = np.diff(gcode, n=1, axis=0)

結果は下記の通りです。

[[ 27.2     0.      0.   ]
 [  0.     27.2     0.   ]
 [-27.2     0.      0.   ]
 ...
 [  0.954  -0.954   0.   ]
 [ -0.566   0.      0.   ]
 [ -0.388   0.388   0.   ]]

タイミングベルト駆動と仮定してパルス数の配列を計算する

タイミングベルトとプーリーで軸を駆動していると仮定して、座標の差分に掛ける係数を求めます。係数はプーリーの歯数とピッチ、モーターの1パルス当たりの角度、マイクロステップ数から求めて、座標の差分の配列に掛ける為にnumpy配列として係数を作成します。そして、この係数を座標の差分に掛けてパルス数の配列を計算します。

def mm_to_pulse_factor_for_belt_pulley(belt_pitch, pulley_tooth, motor_pulse_rate, microstep=1):
    '''
    belt_pitch [mm]
    pulley_tooth [T]
    motor_pulse_rate [pulse/rev]
    return []
    '''
    feed = belt_pitch * pulley_tooth # mm/rev
    factor = motor_pulse_rate * microstep / feed # [pulse/rev] / [mm/rev] = [pulse/mm]
    return factor

array_length = gcode_diff.shape[0]
factor_array = np.array([
    np.full( array_length, mm_to_pulse_factor_for_belt_pulley(2, 20, 200, 16) ),
    np.full( array_length, mm_to_pulse_factor_for_belt_pulley(2, 20, 200, 16) ),
    np.full( array_length, mm_to_pulse_factor_for_belt_pulley(2, 20, 200, 16) )
]).T

pulse_array = gcode_diff * factor_array

計算結果は以下の通りで、1つ目の要素がXを2176パルス、2つ目の要素がYの2176パルスというような内容の配列です。

[[ 2176.       0.       0.  ]
 [    0.    2176.       0.  ]
 [-2176.       0.       0.  ]
 ...
 [   76.32   -76.32     0.  ]
 [  -45.28     0.       0.  ]
 [  -31.04    31.04     0.  ]]

パルス数の配列をパルスの配列にする

パルス数を01のパルスの配列に変換するために下記の関数を作成しました。

def pulse_count_to_pulse_array(pulse_count):
    count = int(max(np.abs(pulse_count)))
    if count == 0:
        return np.array([[0], [0], [0]])
    index = np.arange(count)
    arr = []
    for i in pulse_count:
        if i == 0:
            arr.append( np.zeros(count, np.int8) )
        else:
            tmp = np.mod( index, round(count / abs(i), 0) )
            tmp_max = tmp.max()
            if tmp_max == 0:
                arr.append( np.full(count, 1 * np.sign(i), np.int8) )
            else:
                tmp = tmp / tmp.max() * np.sign(i)
                arr.append( tmp.astype(np.int8) )
    return np.array(arr)

そしてこの関数で作成したパルスの配列を下記のように結合して目的の配列を作成しました。

pulse_array = np.concatenate([ pulse_count_to_pulse_array(pulse_count) for pulse_count in pulse_counts], 1)

作成した配列は下記の通りです。

[[ 1  1  1 ... -1 -1 -1]
 [ 0  0  0 ...  1  1  1]
 [ 0  0  0 ...  0  0  0]]

ソースコード

import numpy as np

class Gcode(object):

    def __init__(self, text):
        self.data = []
        self.count = 0
        self.lines = text.split('\n')
        self.start_lines, self.print_lines, self.end_lines = self.split_lines()

    def split_lines(self):
        def m107count(lines):
            count = 0
            for i, line in enumerate(lines):
                key = line.split(' ')[0]
                if key == 'M107':
                    count = count + 1
                if count > 1:
                    return i
        
        start = m107count(self.lines) + 1
        end = len(self.lines) - m107count(self.lines[::-1]) - 1

        start_lines = self.lines[:start]
        print_lines = self.lines_to_ndarray(self.lines[start:end])
        end_lines = self.lines[end:]

        return start_lines, print_lines, end_lines

    def lines_to_ndarray(self, print_lines):
        arr = [[0, 0, 0, 0, 0, 0]]
        keys = []

        for line in print_lines:
            splited = line.split(' ')
            d = { s[0].strip() : float(s[1:]) for s in splited[1:] }
            a = []
            for i, key in enumerate(['X', 'Y', 'Z', 'E', 'F', 'S']):
                if key in d:
                    a.append( d.get(key) )
                else:
                    a.append( arr[-1][i] )
            keys.append( splited[0] )
            arr.append(a)

        return [ keys, np.array(arr[1:]) ]

def mm_to_pulse_factor_for_belt_pulley(belt_pitch, pulley_tooth, motor_pulse_rate, microstep=1):
    '''
    belt_pitch [mm]
    pulley_tooth [T]
    motor_pulse_rate [pulse/rev]
    return []
    '''
    feed = belt_pitch * pulley_tooth # mm/rev
    factor = motor_pulse_rate * microstep / feed # [pulse/rev] / [mm/rev] = [pulse/mm]
    return factor

def get_pulse_counts_from_gcode(filepath):
    with open(filepath, 'r') as f:
        gcode = Gcode( f.read() )

    # delete other than xyz
    gcode = np.delete( gcode.print_lines[1], [3, 4, 5], 1 )

    # diff elements
    gcode_diff = np.diff(gcode, n=1, axis=0)
    
    # convert factor mm to pulse count
    array_length = gcode_diff.shape[0]
    factor_array = np.array([
        np.full( array_length, mm_to_pulse_factor_for_belt_pulley(2, 20, 200, 16) ),
        np.full( array_length, mm_to_pulse_factor_for_belt_pulley(2, 20, 200, 16) ),
        np.full( array_length, mm_to_pulse_factor_for_belt_pulley(2, 20, 200, 16) )
    ]).T

    # mm to pulse count
    pulse_array = gcode_diff * factor_array

    return np.round(pulse_array)

def pulse_count_to_pulse_array(pulse_count):
    count = int(max(np.abs(pulse_count)))
    if count == 0:
        return np.array([[0], [0], [0]])
    index = np.arange(count)
    arr = []
    for i in pulse_count:
        if i == 0:
            arr.append( np.zeros(count, np.int8) )
        else:
            tmp = np.mod( index, round(count / abs(i), 0) )
            tmp_max = tmp.max()
            if tmp_max == 0:
                arr.append( np.full(count, 1 * np.sign(i), np.int8) )
            else:
                tmp = tmp / tmp.max() * np.sign(i)
                arr.append( tmp.astype(np.int8) )
    return np.array(arr)

def main():

    pulse_counts = get_pulse_counts_from_gcode('programs/data/XYZcube.gco')
    
    pulse_array = np.concatenate([ pulse_count_to_pulse_array(pulse_count) for pulse_count in pulse_counts], 1)

    print(pulse_array)

if __name__ == '__main__':
    main()

記事の共有

関連記事

コメント

comments powered by Disqus