NumpyでGコードをパルスの配列に変換してみた
2021/01/21 categories:Python| tags:Python|G code|
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()
