Python-LEGO Mindstormsでキー入力操作をしてみる

今回の記事は筆者が細部まで理解できていないためひとまず備忘録として残す。

今回はキーボードでLEGO Mindstorms EV3(以降EV3)を操作するプログラムを作成していく。
Pythonでのキーボード入力ではinputなど様々なメソッドやライブラリがあるが、今回はプログラムがキー入力待ちをせず動的にキー入力を判断させたいためtermiosやfcntlというものを利用していく。

キー入力によって車型のEV3をラジコンのように(正確には違うが)操作できるようプログラムを作成する。

EV3について

教育版 LEGO® MINDSTORMS EV3

本記事内での環境

• PC
  Windows10
  Python 3.7.3
  開発環境 VisualStudioCode

• EV3
  ev3dev
  APIリファレンス

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

今回利用するEV3のモデル

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

termios fcntl

今後追記予定
以下参考記事

• Pythonでキー入力待ちをしないキー入力
• Pythonでキー入力を検出する(Enter無しで)

ソースコード

荒削りなソースコードになるが、以下がソースコードになる。

key_ev3.py

import os
import sys
import termios
import time
import fcntl
from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C
from ev3dev2.button import Button


class Key:
    def key_get(self, non_blocking):
        if non_blocking:
            #標準入力のファイルディスクリプタを取得
            self.fd = sys.stdin.fileno()

            #fdの端末属性をゲットする
            #oldとnewには同じものが入る。
            #newに変更を加えて、適応する
            #oldは、後で元に戻すため
            self.old = termios.tcgetattr(self.fd)
            self.new = termios.tcgetattr(self.fd)

            #new[3]はlflags
            #ICANON(カノニカルモードのフラグ)を外す
            self.new[3] &= ~termios.ICANON
            #ECHO(入力された文字を表示するか否かのフラグ)を外す
            self.new[3] &= ~termios.ECHO

            try:
                self.fcntl_old = fcntl.fcntl(self.fd, fcntl.F_GETFL)
                fcntl.fcntl(self.fd, fcntl.F_SETFL, self.fcntl_old |
                            os.O_NONBLOCK)

                # 書き換えたnewをfdに適応する
                termios.tcsetattr(self.fd, termios.TCSANOW, self.new)
                # キーボードから入力を受ける。
                # lfalgsが書き換えられているので、エンターを押さなくても次に進む。echoもしない
                self.ch = sys.stdin.read(1)
            finally:
                fcntl.fcntl(self.fd, fcntl.F_SETFL, self.fcntl_old)
                # fdの属性を元に戻す
                # 具体的にはICANONとECHOが元に戻る
                termios.tcsetattr(self.fd, termios.TCSANOW, self.old)
            return self.ch
        else:
            self.fd = sys.stdin.fileno()
            self.old = termios.tcgetattr(self.fd)
            self.new = termios.tcgetattr(self.fd)
            self.new[3] &= ~termios.ICANON
            self.new[3] &= ~termios.ECHO
            try:
                termios.tcsetattr(self.fd, termios.TCSANOW, self.new)
                self.ch = sys.stdin.read(1)
            finally:
                termios.tcsetattr(self.fd, termios.TCSANOW, self.old)
            return self.ch


    def key_control(self, throttle, steer, non_blocking = True):
        self.lm_b = LargeMotor(OUTPUT_B)
        self.lm_c = LargeMotor(OUTPUT_C)
        self._key = self.key_get(non_blocking)
        if self._key == 'w' and throttle < 1000:  # throttle up
            throttle += 100
        elif self._key == 's' and throttle > -1000:  # throttle down
            throttle -= 100
        elif self._key == 'r':  # reset steer
            steer = 0
        elif self._key == 'a' and steer < 100:  # right
            steer += 10
        elif self._key == 'd' and steer > -100:  # left
            steer -= 10
        elif self._key == 'q':  # stop
            throttle = 0
        else:
            pass
        return throttle, steer


    def run(self, throttle, steer):
        # throttle range:-1050 -> 1050, steer range:-100 -> 100
        if steer < 0:
            self.lm_b.run_forever(speed_sp = throttle * (1 + steer / 50))
            self.lm_c.run_forever(speed_sp = throttle)
        elif steer > 0:
            self.lm_b.run_forever(speed_sp = throttle)
            self.lm_c.run_forever(speed_sp = throttle * (1 - steer / 50))
        else:
            self.lm_b.run_forever(speed_sp = throttle)
            self.lm_c.run_forever(speed_sp = throttle)


if __name__ == '__main__':
    throttle = 0
    steer = 0
    button = Button()
    non_blocking = True
    key = Key()
    print('start')
    while not(button.backspace):
        throttle, steer = key.key_control(throttle, steer, non_blocking)
        key.run(throttle, steer)

プログラムをVSCodeで実行後、実行したターミナル上でキーボードのWASDキーを入力しEV3の操作を行う。
以下のような処理になっている。
W：スロットル値を+100
S：スロットル値を-100
A：ステア値を+10
D：ステア値を-10
スロットル値は前進後退するスピードで-1050~1050の間の値が有効である。正の値で前進し負の値で交代する。
ステア値は左右旋回する度合いの値(ハンドルの切り具合)で-100~100の値が有効である。正の値で左旋回し、負の値で右旋回する。

上記を見てもらえばわかるように、今回の実装ではラジコンのように押している間動作するというものではなく徐々に前進や旋回するスピードを調整しながら走行する。
またこれでは走行の対応力が低いため以下のキー操作での処理も加えている。
R：ステア値のリセット(ステア値を0に)
Q：スロットル値をリセット(スロットル値を0に)
Rキーではステア値を0にすることで旋回を瞬時に中断することができ、
Qキーではスロットル値を0にすることで車体を停止することができる。