Pythonでジャイロターン

塩谷和紀くんの作例その２です。

〔ジャイロのプログラム〕

from hub import port,motion_sensor
import motor_pair,motor,color_sensor,math,runloop,time

# ------------------------------
# 初期化（ポート等は環境に合わせてください）
motor_pair.pair(motor_pair.PAIR_1,port.A,port.E)

# ------------------------------
# 定数・設定パラメータ
# ------------------------------

TARGET_REFLECTION = 40.0# 目標反射率（環境に合わせて調整）#(黒の反射-白の反射光)/２
ANGLE = 1
KP = 0.2 #Pの定数
KI = 0.005 #Iの定数
KD = 0.01 #Dの定数

MIN_SPEED = 300# 最低速度
MAX_SPEED = 700# 最高速度

ACC_DISTANCE = 3# 加速区間（cm）
DEC_DISTANCE = 5# 減速区間（cm）
TOTAL_DISTANCE = 100.0# 今回は100cm走行

TURN_THRESHOLD = 1    # ±1°以内ならターン完了とする
TURN_KP = 8    # ターン用比例ゲイン
MIN_TURN_SPEED = 150  # ターン時の最低速度（絶対値）
TARGET_PERIOD = 0.01    # 制御ループ周期（秒）
DRIVE_SPEED = 60

WHEEL_CIRCUMFERENCE = 19.5# ホイールの円周(cm)

speed=500

async def turn_angle(turn_angle,min_turn_speed=MIN_TURN_SPEED):
    """
    turn_angle(turn_angle):
    - turn_angle が正の場合は右旋回、負の場合は左旋回する関数です。
    - ジャイロセンサ（hub.motion_sensor）の符号付き角度を利用して、目標角度に到達するまで旋回します。

    例:
        turn_angle(90)→ 右90°旋回
        turn_angle(-90) → 左90°旋回
    """
    # 旋回開始前にジャイロセンサの角度をリセット
    runloop.sleep_ms(5)# センサ安定のため短時間待つ

    # ここで目標角度はそのまま符号付きで保持する
    target = turn_angle

    while True:
        loop_start = time.ticks_ms()
        # 現在の角度は符号付きで取得
        current_angle = motion_sensor.tilt_angles()[0]*-0.1
        error = target - current_angle# error は符号付き
        #correct_drift(current_angle, target)
        # 目標に近づいているか判定（± TURN_THRESHOLD 内なら完了）
        if abs(error) <= TURN_THRESHOLD:
            break
        # 比例制御：誤差の絶対値に比例したスピードを計算
        turn_speed = TURN_KP * abs(error)
        if turn_speed < min_turn_speed:
             turn_speed = min_turn_speed
        turn_speed = int(turn_speed)
        # エラーが正なら（ターゲット > 現在値）右旋回、負なら左旋回
        if error > 0:
            drive_speed = turn_speed    # 右旋回のため正の値
            steering_value = 100        # 右旋回：steering 正
        else:
            drive_speed = turn_speed    # 左旋回のため正の値を使用（モーター指令は、steering で方向決定）
            steering_value = -100        # 左旋回：steering 負
        motor_pair.move(motor_pair.PAIR_1,int(steering_value),velocity=drive_speed)
        print(drive_speed)
        # 一定周期となるようにループ周期を調整
        loop_elapsed = time.ticks_ms() - loop_start
        sleep_time = TARGET_PERIOD - loop_elapsed
        if sleep_time > 0:
            time.sleep(sleep_time)
    motor_pair.stop(motor_pair.PAIR_1)

async def main():
    motion_sensor.reset_yaw(0)
    await turn_angle(86)
runloop.run(main())

このプログラムはジャイロセンサーのヨー角を使って曲がり、かつ比例制御で減速しながら90度回転します。ジャイロセンサーを使って回転させることで、値を入れた場所に向けて、正確に回転することができるため、その後の動きのズレが少なくなります。

<使い方>
曲がりたい角度のヨー角の値を変えることで、回転の度数を変えることができます。
turn_angle(曲がりたい角度のヨー角の値 ,最低速度。
数値を色々変えてみて、試してみてください。

分からないことがあれば、何でも聞いてください。