ステッピングモーターをmm単位で動かす計算式とPython実装【XYテーブル自作 第3回】

import RPi.GPIO as GPIO
import time

# ===== 機構設定 =====
BELT_PITCH = 2.0　#2GTタイミングベルトによる固定値
PULLEY_TEETH = 32　#32歯プーリーによる固定値
STEPS_PER_REV = 200

MM_PER_REV = BELT_PITCH * PULLEY_TEETH
MM_PER_STEP = MM_PER_REV / STEPS_PER_REV

# ===== PIN設定 =====
X_STEP  = 21
X_DIR   = 20
X_EN    = 16
X_MS1  = 17
X_MS2  = 27
X_MS3  = 22
X_RESET = 5
X_SLEEP = 6

GPIO.setmode(GPIO.BCM)

for pin in [X_STEP, X_DIR, X_EN, X_RESET, X_SLEEP]:
    GPIO.setup(pin, GPIO.OUT)

GPIO.output(X_EN, GPIO.LOW)
GPIO.output(X_RESET, GPIO.HIGH)
GPIO.output(X_SLEEP, GPIO.HIGH)
GPIO.output(X_MS1, GPIO.LOW)
GPIO.output(X_MS2, GPIO.LOW)
GPIO.output(X_MS3, GPIO.LOW)

def move_mm(distance_mm, direction, delay=0.01):    # delayの値で速度調整　値が大きいと遅くなる
    steps = int(distance_mm / MM_PER_STEP)
    GPIO.output(X_DIR, direction)

    for _ in range(steps):
        GPIO.output(X_STEP, GPIO.HIGH)
        time.sleep(delay)
        GPIO.output(X_STEP, GPIO.LOW)
        time.sleep(delay)

try:
    print("20mm 正転")
    move_mm(20, GPIO.LOW)  # 20＝移動量(mm)　GPIO.LOW＝正転
    time.sleep(1)

    print("10mm 逆転")  # 10＝移動量(mm)　GPIO.HIGH＝逆転
    move_mm(10, GPIO.HIGH)

finally:
    GPIO.cleanup()