Python-LEGO MindstormsでPIDライントレース その２

前回はev3dev環境で用意されているメソッドを利用して簡単にPID制御を用いてライントレースする内容だった。
用意されているメソッドを利用すると簡単に実装できるが、センサーなどを併用した処理ができなかったので、今回は自力でPID制御を実装していく。

EV3について

教育版 LEGO® MINDSTORMS EV3

本記事内での環境

• PC
  Windows10
  Python 3.7.3
  開発環境 VisualStudioCode

• EV3
  ev3dev
  APIリファレンス

環境構築やソースコードの作成、実行はこちら

今回利用するEV3のモデル

今回も前回に引き続きEV3を走行させるためにベースロボというモデルでモーターを利用して走行を行う。

これに加え今回はタッチセンサーも利用する。

参考

• ArduinoでSTEM教育​ 実践編：ライントレースカーをPID制御する
• pythonを使ってev3をp制御

ソースコード

pid-2.py

from ev3dev2.motor import OUTPUT_B, OUTPUT_C, LargeMotor, MoveSteering, SpeedPercent
from ev3dev2.sensor.lego import ColorSensor, TouchSensor
import time

touch = TouchSensor()
color = ColorSensor()
lm_b = LargeMotor(OUTPUT_B)
lm_c = LargeMotor(OUTPUT_C)
stm = MoveSteering(OUTPUT_B, OUTPUT_C)


#カラーセンサーによる白黒の反射光取得
def get_bw():
    global black, white
    print('push for white')
    while not touch.is_pressed:
        pass
    white = color.reflected_light_intensity
    print('white intensity: {}'.format(white))
    while touch.is_pressed:
        pass

    print('push for black')
    while not touch.is_pressed:
        pass
    black = color.reflected_light_intensity
    print('black intensity: {}'.format(black))
    while touch.is_pressed:
        pass
    return black, white



#PID走行をする関数
def pid_run(kp_value, ki_value, kd_value, b, w):
    global last_error
    pid_value = 0
    midpoint = (b + w) / 2
    last_error = 0
    cs = color.reflected_light_intensity
    error = cs - midpoint

    pid_value = kp_value * error + ki_value * (error + last_error) + kd_value * (error - last_error)
    stm.on(pid_value, SpeedPercent(20))

    last_error = error



def main():
    get_bw()
    while not touch.is_pressed:
        pid_run(0.5, 0.3, 0.5, black, white)
    lm_b.stop(stop_action='brake')
    lm_c.stop(stop_action='brake')



if __name__ == '__main__':
    main()

get_bw()で走行前にライントレースコースの白部分の反射光値と黒線部分の反射光値を取得する。取得した白と黒の反射光値はpid_run()の引数b,wに入れられている。
前回の用意されているメソッドを利用したPID走行ではメソッドから処理が抜け出せなかったため、センサーなどによる割り込み判定などができなかった。今回はmain()にあるとおり、タッチセンサーが押されるまでPID走行を継続し、押されたらwhileループから抜け出し走行を終了するような処理になっている。