LEGO MINDSTORMS でステートマシン

なんちゃってスレッドができるようになりましたが、プログラムを勉強していくとまずはモジュール化という考え方が必要になってきます。

Scratchではスプライトというオブジェクト毎にプログラムを作成するということで自然とモジュール化されるようになっていますし、ブロック化やメッセージ送信・受信ということで意識的にモジュール化することも可能です。

私がプログラミングを始めた頃は構造化プログラミングという言葉が流行っていましたがその後、オブジェクト指向、イベント駆動、データ駆動、ドメイン駆動にテスト駆動とプログラミング界隈でもいろいろな言葉が流行りましたが、基本は構造化プログラミングかなと個人的には思っています。

おそらくなんでもそうなんでしょうがある程度のところまでは才能だけで行くことが出来ますが、その上に行くにはやはり基本が大切なのかなと思います。

以下にMINDSTORMSのMicroPythonでのステートマシンのプログラム例を示しますが、ステートマシンは複雑な振る舞いの制御によく使われていて、ROS(Robot Operating System)ではsmachパッケージとしてPythonで利用できるようになっています。

MINDSTORMS用ステートマシン

必要に応じてuasyncioを利用した非同期処理を組み込めば複雑な処理も可能になりますが、その場合はイベント（uasyncuo.Event）によるタスク同期なども考える必要が出てきます。

from mindstorms import MSHub, Motor, MotorPair, ColorSensor, DistanceSensor, App
from mindstorms.control import wait_for_seconds, wait_until, Timer
from mindstorms.operator import greater_than, greater_than_or_equal_to, less_than, less_than_or_equal_to, equal_to, not_equal_to
import math

# オブジェクトの定義
app = App()
hub = MSHub()
wheels = MotorPair(MSHub.PORT_B, MSHub.PORT_A)
arm = Motor(MSHub.PORT_C)
distance_sensor = DistanceSensor(MSHub.PORT_D)
color_sensor = ColorSensor(MSHub.PORT_E)

# ステートの定義
STATE_EXIT = "exit"
STATE_GO_STRAIGHT = "GoStraight"

# ステートクラス（基底クラス）
class State(object):
    def __init__(self, name: String):
        self.__name = name
        self.__action = None

    @property
    def name(self):
        return self.__name

    def entry(self):
        pass

    def do(self) -> String:
        return None

    def exit(self):
        pass

# ステートマシンクラス
class StateMachine(object):
    def __init__(self):
        self.__stateList = {}
        self.__now = None

    @property
    def currentState(self):
        return self.__now.name if self.__now else ""

    def add(self, state: State):
        self.__stateList[state.name] = state

    def changeTo(self, tag: String):
        if self.__now:
            self.__now.exit()
        self.__now = self.__stateList[tag]
        if self.__now:
            self.__now.entry()

    def run(self) -> String:
        if self.__now:
            return self.__now.do()
        return None

# 直進
class GoStraight(State):
    def __init__(self, name: String = STATE_GO_STRAIGHT):
        super().__init__(name)

    def entry(self):
        wheels.start(0, -20)

    def do(self) -> String:
        distance = distance_sensor.get_distance_cm()
        if distance and distance < 20: # 測定不能の場合None
            return STATE_EXIT
        return None

    def exit(self):
        wheels.stop()

# メイン処理
class Main:
    def __init__(self):
        self.__stateMachine = StateMachine()
        self.__stateMachine.add(State(STATE_EXIT))
        self.setup()
        self.loop()

    def setup(self):
        # ステートの登録
        self.__stateMachine.add(GoStraight())
        # 開始ステートへ切り替える
        self.__stateMachine.changeTo(STATE_GO_STRAIGHT)
        
    def loop(self):
        while self.__stateMachine.currentState != STATE_EXIT:
            nextState = self.__stateMachine.run()
            if nextState:
                self.__stateMachine.changeTo(nextState)
            # 外部よりのステートマシン終了
            if hub.right_button.is_pressed():
                print("right button is pressed !")
                self.__stateMachine.changeTo(STATE_EXIT)

# プログラム開始指示を待つ
hub.left_button.wait_until_pressed()
hub.speaker.beep()
# プログラム開始
Main()
# プログラム終了（メニューに戻る）
raise SystemExit