Generate pulse arrays from G code and drive stepping motors with Raspberry Pi

I added a Raspberry Pi GPIO output program to the Convert Gcode to arrays of pulses with Numpy program and tried to drive the stepping motor based on the G code.

Circuit configuration

The image of the circuit configuration is as follows, assuming 2-axis drive of XY axes.

GPIO output

I used pigpio for the IO output of Raspberry Pi and DRV8825 for the motor driver. There are two inputs to the driver, STEP and DIR, and both IO of Raspberry Pi are set to output. This time we will move the 2 axes of XY, so we will use 4 IOs. The IO input / output settings are as follows.

pi = pigpio.pi()
pi.set_mode(X, pigpio.OUTPUT)
pi.set_mode(Y, pigpio.OUTPUT)
pi.set_mode(X_DIR, pigpio.OUTPUT)
pi.set_mode(Y_DIR, pigpio.OUTPUT)

The output of the pulse was simply output with time.sleep () with wait. The wait time may not be constant, but as a test I tried to generate a pulse with time.sleep (). The processing of one pulse output is as follows.

x_val, y_val = pulse[0], pulse[1]
pi.write( X_DIR, int((np.sign(x_val) + 1) / 2) )
pi.write( Y_DIR, int((np.sign(y_val) + 1) / 2) )
pi.write( X, abs(x_val) )
pi.write( Y, abs(y_val) )
time.sleep(velocity)
pi.write(X, 0)
pi.write(Y, 0)
time.sleep(velocity)

This pulse output process is performed for all pulses to drive the stepping motor.

State of operation

先日作ったGコードをパルス列の配列に変換するプログラムにGPIO出力を追加してRaspberry Pi 4 Model BとDRV8825とNEMA17のステッピングモータで動作させたら3Dプリンタっぽい動きした。簡単なXYロボットのハードを用意してXYの動きとして確認してみよう。 pic.twitter.com/bN9vZoUdWX
— ymt-lab (@ymt_lab) January 23, 2021

Source code

import numpy as np
import pigpio, time

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, 1) ),
        np.full( array_length, mm_to_pulse_factor_for_belt_pulley(2, 20, 200, 1) ),
        np.full( array_length, mm_to_pulse_factor_for_belt_pulley(2, 20, 200, 1) )
    ]).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 move_motor(X, Y, X_DIR, Y_DIR, pulse_array, velocity):
    pi = pigpio.pi()
    pi.set_mode(X, pigpio.OUTPUT)
    pi.set_mode(Y, pigpio.OUTPUT)
    pi.set_mode(X_DIR, pigpio.OUTPUT)
    pi.set_mode(Y_DIR, pigpio.OUTPUT)

    try:
        print(pulse_array.shape)
        for pulse in pulse_array:
            x_val, y_val = pulse[0], pulse[1]
            pi.write( X_DIR, int((np.sign(x_val) + 1) / 2) )
            pi.write( Y_DIR, int((np.sign(y_val) + 1) / 2) )
            pi.write( X, abs(x_val) )
            pi.write( Y, abs(y_val) )
            time.sleep(velocity)
            pi.write(X, 0)
            pi.write(Y, 0)
            time.sleep(velocity)
            #print(pulse)
    except(KeyboardInterrupt): # when ctrl+c is pressed
        pi.write(X, 0)
        pi.write(Y, 0)
        pi.stop()
        print('\nstop motor')

def main():
    print('calcurate pulse array')
    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)
    pulse_array = pulse_array.T

    print('move motor')
    move_motor(17, 27, 22, 23, pulse_array, 0.002)

if __name__ == '__main__':
    main()