ラズパイでステッピングモーターを2軸制御する方法【XYテーブル自作 第4回】

import RPi.GPIO as GPIO
import time

# =====================================================
# GPIO モード設定
# =====================================================
GPIO.setmode(GPIO.BCM)

# =====================================================
# X軸 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

# =====================================================
# Y軸 PIN設定
# =====================================================
Y_STEP  = 2
Y_DIR   = 26
Y_EN    = 18
Y_MS1   = 23
Y_MS2   = 24
Y_MS3   = 25
Y_RESET = 12
Y_SLEEP = 13

# =====================================================
# モーション設定（将来互換）
# =====================================================
BELT_PITCH = 2.0
PULLEY_TEETH = 32
FULL_STEPS_PER_REV = 200
MICROSTEP = 16

MM_PER_REV = BELT_PITCH * PULLEY_TEETH
STEPS_PER_REV = FULL_STEPS_PER_REV * MICROSTEP
STEPS_PER_MM = STEPS_PER_REV / MM_PER_REV


# =====================================================
# GPIO 初期化関数（rev4互換）
# =====================================================
def gpio_init_axis(STEP, DIR, EN, MS1, MS2, MS3, RESET, SLEEP):

    pins = [STEP, DIR, EN, MS1, MS2, MS3, RESET, SLEEP]

    for pin in pins:
        GPIO.setup(pin, GPIO.OUT)

    GPIO.output(RESET, GPIO.HIGH)
    GPIO.output(SLEEP, GPIO.HIGH)
    GPIO.output(EN, GPIO.LOW)

    # 1/16 マイクロステップ
    GPIO.output(MS1, GPIO.HIGH)
    GPIO.output(MS2, GPIO.HIGH)
    GPIO.output(MS3, GPIO.HIGH)


# =====================================================
# 軸移動関数（一定速度）
# =====================================================
def move_axis(axis_name,
              STEP, DIR, EN,
              MS1, MS2, MS3,
              RESET, SLEEP,
              distance_mm):

    gpio_init_axis(STEP, DIR, EN, MS1, MS2, MS3, RESET, SLEEP)

    steps = int(distance_mm * STEPS_PER_MM)

    if steps <= 0:
        print(f"{axis_name}：移動量が小さすぎます")
        return

    print(f"{axis_name} を {distance_mm} mm 移動します")

    # 軸ごとの正転方向（rev4と合わせる）
    if axis_name == "X軸":
        GPIO.output(DIR, GPIO.LOW)
    else:
        GPIO.output(DIR, GPIO.HIGH)

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

    GPIO.output(EN, GPIO.HIGH)

    print(f"{axis_name} 移動完了")


# =====================================================
# メイン処理
# =====================================================
try:
    # X軸 20mm
    move_axis("X軸",
              X_STEP, X_DIR, X_EN,
              X_MS1, X_MS2, X_MS3,
              X_RESET, X_SLEEP,
              20)

    time.sleep(1)

    # Y軸 20mm
    move_axis("Y軸",
              Y_STEP, Y_DIR, Y_EN,
              Y_MS1, Y_MS2, Y_MS3,
              Y_RESET, Y_SLEEP,
              20)

finally:
    GPIO.cleanup()
    print("GPIO cleanup 完了")