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

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

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

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

## EV3について
教育版 [LEGO® MINDSTORMS EV3](https://afrel.co.jp/product/ev3-introduction)

## 本記事内での環境  
* PC
Windows10
Python 3.7.3
開発環境 VisualStudioCode  

* EV3
ev3dev
[APIリファレンス](https://ev3dev-lang.readthedocs.io/projects/python-ev3dev/en/stable/spec.html)

環境構築やソースコードの作成、実行は[こちら](https://qiita.com/Hiroki-Fujimoto/items/6ce278411ca151fee750)

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

## termios fcntl
今後追記予定
以下参考記事

* [Pythonでキー入力待ちをしないキー入力](https://qiita.com/pukin/items/3b791b8b759dd704f765)
* [Pythonでキー入力を検出する(Enter無しで)](https://qiita.com/tortuepin/items/9ede6ca603ddc74f91ba)

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

~~~ 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にすることで車体を停止することができる。