ステッピングモーターの脱調を防ぐ加減速制御をPythonで実装【XYテーブル自作 第5回】

import RPi.GPIO as GPIO
import time

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

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

# =====================================================
# モーション設定（将来互換）
# =====================================================
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
MM_PER_STEP = MM_PER_REV / STEPS_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 get_intervals(total_steps):

    min_delay = 0.003   # 最高速
    max_delay = 0.009    # 最低速

    accel_steps = total_steps // 3
    decel_steps = accel_steps
    const_steps = total_steps - accel_steps - decel_steps

    intervals = []

    # 加速
    for i in range(accel_steps):
        ratio = i / accel_steps
        delay = max_delay - (max_delay - min_delay) * ratio
        intervals.append(delay)

    # 定速
    for i in range(const_steps):
        intervals.append(min_delay)

    # 減速
    for i in range(decel_steps):
        ratio = i / decel_steps
        delay = min_delay + (max_delay - min_delay) * ratio
        intervals.append(delay)

    return intervals


# ===== 移動関数（加減速付き） =====
def move_mm(distance_mm, direction):

    steps = int(distance_mm / MM_PER_STEP)

    GPIO.output(X_DIR, direction)

    intervals = get_intervals(steps)

    for delay in intervals:

        GPIO.output(X_STEP, GPIO.LOW)
        time.sleep(delay)

        GPIO.output(X_STEP, GPIO.HIGH)
        time.sleep(delay)


try:

    print("50mm 正転（加速 → 定速 → 減速）")
    move_mm(50, GPIO.LOW)

    time.sleep(1)

    print("50mm 逆転（加速 → 定速 → 減速）")
    move_mm(50, GPIO.HIGH)

finally:

    GPIO.cleanup()

delay = max_delay - (max_delay - min_delay) * ratio   # 加速：徐々に短く（速く）
delay = min_delay + (max_delay - min_delay) * ratio   # 減速：徐々に長く（遅く）