今回の記事は筆者が細部まで理解できていないためひとまず備忘録として残す。
今回はキーボードでLEGO Mindstorms EV3(以降EV3)を操作するプログラムを作成していく。
Pythonでのキーボード入力ではinputなど様々なメソッドやライブラリがあるが、今回はプログラムがキー入力待ちをせず動的にキー入力を判断させたいためtermiosやfcntlというものを利用していく。
キー入力によって車型のEV3をラジコンのように(正確には違うが)操作できるようプログラムを作成する。
EV3について
本記事内での環境
-
PC
Windows10
Python 3.7.3
開発環境 VisualStudioCode -
EV3
ev3dev
APIリファレンス
環境構築やソースコードの作成、実行はこちら
今回利用するEV3のモデル
今回はEV3を走行させるためにベースロボというモデルでモーターを利用して走行を行う。
termios fcntl
今後追記予定
以下参考記事
ソースコード
荒削りなソースコードになるが、以下がソースコードになる。
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にすることで車体を停止することができる。