Pythonロボティクスコース レッスン 32

Pythonロボティクスコース レッスン 32

テーマ.8-3 カラーセンサーを使用した運搬物の識別

運搬物を色で識別する機能を追加しよう！

チャプター1

このレッスンで学ぶこと

このレッスンでは、レッスン30・31で製作したロボットアームに新たにカラーセンサーを追加して、運搬物を色で識別して運ぶ機能を作成します。

チャプター2

カラーセンサーの追加

レッスン31のロボットアームへ、カラーセンサーを取り付けます。下の組立説明書を確認して、製作しましょう。

2. 1 組み立てに必要なパーツ

【 パーツ一覧 】
  • レッスン31で製作した赤外線フォトリフレクタ付きのロボットアーム×1
  • カラーセンサー×1
  • センサー接続コード（4芯30cm）×1
  • ブロックハーフA（グレー）×1
  • ブロックハーフC（白）×1
【 アーテックブロックの形状 】

2. 2 組立説明書

以下のリンク先から組立説明書を確認しましょう。

カラーセンサーの取り付け方

チャプター3

ブロックの色を検出して運ぶプログラムの作成

今回は、取り付けたカラーセンサーで持ち上げたブロックの色を読み取り、同じ色の円まで運ぶ機能を作成します。もし、色を識別できなかった場合は、黒の円へ運びます。

3. 1 RobotArmクラスの拡張

この機能を作成するために、レッスン31で拡張したRobotArmクラスへ新たに次のプロパティとメソッドを追加します。

【 RobotArmクラスに追加するプロパティ 】
プロパティ名内容
csColorSensorクラスのインスタンス
COLOR_POSITONS円の色と位置を示す番号を関連付けた辞書型のデータ
【 RobotArmクラスに追加するメソッド 】
メソッド名内容
recognize_color()持ち上げたブロック色を読み取り、その色名を返す
move_to_color()指定された色の円へボディを回転して移動する

では、これらのプロパティとメソッドを順番に定義していきましょう。

■ csプロパティの定義

上記のプロパティやメソッドは、前のレッスンで定義したRobotArmクラスに追加していきます。もし、プログラムを削除してしまった場合は、以下のコードをコピーして新しいファイルに貼り付けましょう。

【 レッスン31で機能を拡張したRobotArmクラスの定義 】
class RobotArm:
    from pyatcrobo2.parts import DCMotor, IRPhotoReflector
    import myservo
    import time

    threshold_high = 1200
    threshold_low = 900

    def __init__(self, *, pin_arm, pin_hand, pin_body, pin_irp):
        self.arm = self.myservo.MyServomotor(pin_arm)
        self.hand = self.myservo.MyServomotor(pin_hand)
        self.body = self.DCMotor(pin_body)
        self.irp = self.IRPhotoReflector(pin_irp)
        self.position = 1

    def pick(self):
        self.myservo.syncRotateServos(600, (self.arm, 165), (self.hand, 135))
        self.myservo.syncRotateServos(600, (self.hand, 60))
        self.myservo.syncRotateServos(600, (self.arm, 90))

    def place(self):
        self.myservo.syncRotateServos(600, (self.arm, 165))
        self.myservo.syncRotateServos(600, (self.hand, 135))
        self.myservo.syncRotateServos(600, (self.arm, 90), (self.hand, 90))

    def rotate(self, duration=-1, speed=50, direction="cw"):
        self.body.power(speed)
        if direction == "cw":
            self.body.cw()
        else:
            self.body.ccw()
        if duration != -1:
            self.time.sleep_ms(duration)
            self.body.brake()

    def stop(self):
        self.body.brake()

    def move_to(self, num):
        diff = num - self.position
        if abs(diff) > 3:
            diff = diff - 6 if diff > 0 else diff + 6
        direction = "cw" if diff > 0 else "ccw"
        self.rotate(direction=direction)
        for _ in range(abs(diff)):
            while not self.irp.get_value() > self.threshold_high:
                pass
            while not self.irp.get_value() < self.threshold_low:
                pass
        self.stop()
        self.position = num

では、追加したカラーセンサーを使用するために、ColorSensorクラスのインスタンスを作成して、csプロパティに格納します。ColorSensorクラスをインポートし、__init__()メソッドで受け取った端子名からインスタンスを作成しましょう。

変更・追加【2行目、9行目、14行目】
class RobotArm:
    from pyatcrobo2.parts import DCMotor, IRPhotoReflector, ColorSensor  # ColorSensorクラスを追加
    import myservo
    import time

    threshold_high = 1200
    threshold_low = 950

    def __init__(self, *, pin_arm, pin_hand, pin_body, pin_irp, pin_cs):  # カラーセンサーの端子名を受け取る
        self.arm = self.myservo.MyServomotor(pin_arm)
        self.hand = self.myservo.MyServomotor(pin_hand)
        self.body = self.DCMotor(pin_body)
        self.irp = self.IRPhotoReflector(pin_irp)
        self.cs = self.ColorSensor(pin_cs)  # ColorSensorクラスのインスタンス作成
        self.position = 1

■ COLOR_POSITIONSプロパティの定義

レッスン31で、move_to()メソッドを定義するために、各円に対して１～６番までの番号を割り振りました。

今回は番号ではなく、色を指定して移動できるようにするため、番号と色名の対応付けをした辞書を用意します。COLOR_POSITIONSという名前で以下のように定義しましょう。

追加【6行目～13行目】
class RobotArm:
    from pyatcrobo2.parts import DCMotor, IRPhotoReflector, ColorSensor
    import myservo
    import time

    COLOR_POSITIONS = {
        "grey": 1,
        "red": 2,
        "green": 3,
        "undef": 4,
        "blue": 5,
        "yellow": 6,
    }

    threshold_high = 1200
    threshold_low = 950

■ recognize_color()メソッドの定義

色の識別には、ColorSensorクラスに用意されているget_colorcode()メソッドを使用します。このメソッドの戻り値は、同じくColorSensorクラスで用意されている以下のプロパティです。

【 get_colorcode()メソッドの戻り値 】の一覧
識別された色の名前戻り値（ColorSensorクラスのプロパティ）
赤COLOR_RED
緑COLOR_GREEN
青COLOR_BLUE
白COLOR_WHITE
黄COLOR_YELLOW
オレンジCOLOR_ORANGE
紫COLOR_PURPLE
不明COLOR_UNDEF

recognize_color()メソッドでは、get_colorcode()メソッドで取得したプロパティから色名の文字列へ変換し、戻り値として返します。今回は、運搬の対象となる「赤（"red"）、緑（"green"）、青（"blue"）、黄（"yellow"）」のみ色名の文字列を用意し、それ以外は不明な色として、"undef"を返すようにしましょう。

追加【63行目～75行目】
    def move_to(self, num):
        diff = num - self.position
        if abs(diff) > 3:
            diff = diff - 6 if diff > 0 else diff + 6
        direction = "cw" if diff > 0 else "ccw"
        self.rotate(direction=direction)
        for _ in range(abs(diff)):
            while not self.irp.get_value() > self.threshold_high:
                pass
            while not self.irp.get_value() < self.threshold_low:
                pass
        self.stop()
        self.position = num

    def recognize_color(self):  # 色を識別するメソッド
        color = self.cs.get_colorcode()  # ColorSensorクラスのプロパティとして定義された色情報を取得
        if color == self.cs.COLOR_RED:  # 赤の場合
            color_name = "red"
        elif color == self.cs.COLOR_GREEN:  # 緑の場合
            color_name = "green"
        elif color == self.cs.COLOR_BLUE:  # 青の場合
            color_name = "blue"
        elif color == self.cs.COLOR_YELLOW:  # 黄の場合
            color_name = "yellow"
        else:  # 上記以外（COLOR_WHITE、COLOR_ORANGE、COLOR_PURPLE、COLOR_UNDEF）の場合
            color_name = "undef"
        return color_name

■ move_to_color()メソッドの定義

move_to_color()メソッドの定義は簡単です。色名を引数として受け取り、それをCOLOR_POSITIONSプロパティで番号に変換し、move_to()メソッドを実行するだけです。

追加【77行目～79行目】
    def recognize_color(self):
        color = self.cs.get_colorcode()
        if color == self.cs.COLOR_RED:
            color_name = "red"
        elif color == self.cs.COLOR_GREEN:
            color_name = "green"
        elif color == self.cs.COLOR_BLUE:
            color_name = "blue"
        elif color == self.cs.COLOR_YELLOW:
            color_name = "yellow"
        else:
            color_name = "undef"
        return color_name

    def move_to_color(self, color):  # 指定された色の場所まで移動するメソッド
        num = self.COLOR_POSITIONS[color]  # 色名から番号への変換
        self.move_to(num)  # 番号を指定して移動

3. 2 動作の確認

拡張したRobotArmクラスの動作を確認します。まずは、新たに追加した機能を使う前に、ブロックを持ち上げるときのサーボモーターの角度を調整しましょう。

■ サーボモーターの角度調整

カラーセンサーを使用した色の識別は、カラーセンサーとブロックの距離や相対的な角度によって結果が不安定になる場合があります。そこで、今回はブロックを縦ではなく横向きにして置き、さらにブロックをつかむときにおよそブロックの半分の高さのスペースをカラーセンサーとの間に空けるように調整しましょう。

以下のように、最初にアームを下げるときの角度を165度から170度に変更しましょう。もし、これでもブロックに届かない場合は、少し角度を大きくしてください。

変更【27行目】
    def pick(self):  # ブロックをつかむときのアームの位置を少し下げる
        self.myservo.syncRotateServos(600, (self.arm, 170), (self.hand, 135))
        self.myservo.syncRotateServos(600, (self.hand, 60))
        self.myservo.syncRotateServos(600, (self.arm, 90))

■ 持ち上げたブロックの色を調べて運ぶプログラムの作成

では、動作の確認用に「ボタンAを押すと１番に置いたブロックを持ち上げ、色を識別して運搬する」プログラムを作成します。以下のサンプルコードを参考にしてください。

【 サンプルコード 4-2-1 】
追加【82～96行目】
class RobotArm:
    from pyatcrobo2.parts import DCMotor, IRPhotoReflector, ColorSensor
    import myservo
    import time

    COLOR_POSITIONS = {
        "grey": 1,
        "red": 2,
        "green": 3,
        "undef": 4,
        "blue": 5,
        "yellow": 6,
    }

    threshold_high = 1200
    threshold_low = 950

    def __init__(self, *, pin_arm, pin_hand, pin_body, pin_irp, pin_cs):
        self.arm = self.myservo.MyServomotor(pin_arm)
        self.hand = self.myservo.MyServomotor(pin_hand)
        self.body = self.DCMotor(pin_body)
        self.irp = self.IRPhotoReflector(pin_irp)
        self.cs = self.ColorSensor(pin_cs)
        self.position = 1

    def pick(self):
        self.myservo.syncRotateServos(600, (self.arm, 170), (self.hand, 135))
        self.myservo.syncRotateServos(600, (self.hand, 60))
        self.myservo.syncRotateServos(600, (self.arm, 90))

    def place(self):
        self.myservo.syncRotateServos(600, (self.arm, 165))
        self.myservo.syncRotateServos(600, (self.hand, 135))
        self.myservo.syncRotateServos(600, (self.arm, 90), (self.hand, 90))

    def rotate(self, duration=-1, speed=50, direction="cw"):
        self.body.power(speed)
        if direction == "cw":
            self.body.cw()
        else:
            self.body.ccw()
        if duration != -1:
            self.time.sleep_ms(duration)
            self.body.brake()

    def stop(self):
        self.body.brake()

    def move_to(self, num):
        diff = num - self.position
        if abs(diff) > 3:
            diff = diff - 6 if diff > 0 else diff + 6
        direction = "cw" if diff > 0 else "ccw"
        self.rotate(direction=direction)
        for _ in range(abs(diff)):
            while not self.irp.get_value() > self.threshold_high:
                pass
            while not self.irp.get_value() < self.threshold_low:
                pass
        self.stop()
        self.position = num

    def recognize_color(self):
        color = self.cs.get_colorcode()
        if color == self.cs.COLOR_RED:
            color_name = "red"
        elif color == self.cs.COLOR_GREEN:
            color_name = "green"
        elif color == self.cs.COLOR_BLUE:
            color_name = "blue"
        elif color == self.cs.COLOR_YELLOW:
            color_name = "yellow"
        else:
            color_name = "undef"
        return color_name

    def move_to_color(self, color):
        num = self.COLOR_POSITIONS[color]
        self.move_to(num)

# 動作確認用のプログラム
def main():
    from pystubit.board import button_a, display  # ボタンAとディスプレイのオブジェクトをインポート
    # RobotArmクラスのインスタンスの作成
    robotarm = RobotArm(pin_arm="P13", pin_hand="P14", pin_body="M1", pin_irp="P0", pin_cs="I2C")

    while True:
        if button_a.is_pressed():
            robotarm.pick()  # １．ブロックを持ち上げる
            color = robotarm.recognize_color()  # ２．ブロックの色を識別する
            display.scroll(color, delay=10)  # ３．認識した色名をスクロール表示
            robotarm.move_to_color(color)  # ４．同じ色の円まで移動する
            robotarm.place()  # ５．ブロックを置く
            robotarm.move_to(1)  # ６．最初の位置に戻る  main()

■ 識別に失敗する場合の対処方法

アームを下げる位置を変更して、カラーセンサーの正面と持ち上げたブロック間の距離を調整してください。特に青色のブロックは識別に失敗しやすいため、もし上記の調整でもうまくいかない場合は、recognize_color()メソッドで識別結果が"undef"の場合を"blue"に変更してください。

    def recognize_color(self):
        color = self.cs.get_colorcode()
        if color == self.cs.COLOR_RED:
            color_name = "red"
        elif color == self.cs.COLOR_GREEN:
            color_name = "green"
        elif color == self.cs.COLOR_BLUE:
            color_name = "blue"
        elif color == self.cs.COLOR_YELLOW:
            color_name = "yellow"
        else:
            # color_name = "undef"  # undef から blue に変更
            color_name = "blue"
        return color_name

チャプター4

課題：ランダムに配置されたブロックを同じ色の円に運ぶプログラムの作成

この課題では、２番（赤の円）、３番（緑の円）、５番（青の円）、６番（黄の円）へランダムに配置された「赤・青・緑・黄」のブロックを、それぞれ同じ色の円まで運ぶプログラムを考えます。少し複雑なアルゴリズム（※）となるため、手順が思いつかない場合は、以下の解説を参考にプログラムを作成しましょう。

※ アルゴリズム・・・問題を解くための手順を定式化した形で表現したもの

4. 1 アルゴリズムを考える

■ 一時的にブロックを置いておける場所を確保する

まず、ブロックの配置場所を入れ替えるためには、一時的にブロックを置いておける空き場所が１つ必要です。ここでは、黒色の円を最初の空き場所として確保し、入れ替えのときに使います。

■ ブロックを運ぶときの状況を考える

ブロックを運ぶときの状況は、以下の3通りに分かれます

  1. 持ち上げたブロックの色とそれが置かれていた場所の色が一致する場合
  2. 1ではないが、持ち上げたブロックの色と同じ色の場所が空いている場合
  3. 1でも2でもない場合

これら1～3の状況では、それぞれ以下のように次の動作を行うことになります。

  • 1のとき
    ブロックを移動させる必要がないので、その場で置き直す。
  • 2のとき
    ブロックと同じ色の場所まで移動して置く。
  • ３のとき
    ブロックを一旦空いている場所に置く。

ここで注意したいのは3のときです。 １や２はブロックを置いた時点で、ブロックの色と置いた場所の色が一致しているため、それで設置完了となります。しかし、３の場合は色が一致していないため、後で運び直す必要があります。このように後で運び直す必要があるブロックは「運搬待ち」の状態にあるものとして、情報を管理しておきます。

以上のことを踏まえると、このプログラムでは１つブロックを運ぶたびに、次の３つの情報を更新する必要があることに気づきます。

  • ブロックの設置が完了できていない場所
    すべてのブロックの設置が完了できたかどうかを確認するための情報
  • ブロックがなく、空いている場所
    持ち上げたブロックの運搬先となる情報
  • 運搬待ち状態のブロックとその場所
    運搬待ち状態にあるブロックを管理するための情報

そこで、以下の3つの変数を用意してこれらの情報を管理します。

変数名格納するデータ
incomplete設置が完了できていない場所のリスト
freeブロックがなく空いている場所の色名
queue運搬待ち状態のブロックとその場所のリスト（待ち行列）
※incomplete（インコンプリート）は英語で「未完了」という意味があり、queue(キュー)は「順番を待つ列」という意味があります。

■ 処理の手順を考える

最後にプログラムで行う処理の手順を考えてみましょう。

まず、この課題のゴールは、すべてのブロックを同じ色の場所に設置することです。設置が完了するまで、運搬の処理を繰り返し行い、設置が完了できれば繰り返しを終えることになります。

その運搬処理では、始めに運搬待ちのブロックがある場合と、そうでない場合で移動先を変えます。さらに、運搬待ちのブロックがある場合は、そのブロックの色と、空き場所の色が一致するかどうかでも移動先を変えます。そして、どの場合でも移動先でブロックを持ち上げます。

次に、持ち上げたブロックの色がすでに分かっている場合（運搬待ちのブロックの場合、過去に一度色を識別している）と、分かっていない場合で処理を分けます。

ブロックの色が分かっていない場合は、カラーセンサーで色を調べ、LEDディスプレイに識別結果を表示します。そして、移動先の色と持ち上げたブロックの色が一致していた場合は、運搬する必要がないので、そのまま置き直し、この場所を設置未完了のリストから削除します。

色が一致していなかった場合は、ブロックの色が分かっている場合と同様に、持ち上げているブロックを空き場所へと運びます。

最後に、その空き場所とブロックの色が一致しているかどうかで処理を分けます。

色が一致していた場合はその場所を設置未完了のリストから削除します。反対に、色が一致していなかった場合は、ブロックの色とその場所をまとめて、運搬待ちのリスト追加します。

そして、運んだブロックが元々あった場所が新たな空き場所に変わったため、変数の情報を変更します。

以上の流れをすべてつなぐと、以下のような手順となります。

■ 具体例で考えたアルゴリズムを検証する

では、実際に具体的例として以下のケースに沿って、考えたアルゴリズムで正しく作業が完了できるかどうかを検証してみましょう。

【 置き方の例 】
【 アルゴリズムに沿ったロボットアームの動作 】
  1. 設置未完了リストの先頭の番号を選択し、そこに置かれているブロックの色（"blue"）を確認して空き場所（"undef"）へ移動する。ブロックの色と空き場所の色が一致していないため、待ち行列に情報("blue", "undef")を追加する。そして、赤い円（"red"）が新たな空き場所となる。
  2. 待ち行列リストの先頭の情報を確認し、ブロックの色("blue")と空き場所の色("red")が一致しないため、ブロックの色と同じ場所("blue")へ移動し、そこにあるブロック（"yellow"）を空き場所("red")へ運ぶ。運んだブロックの色と空き場所が一致していないため、待ち行列に情報("yellow", "red")を追加する。そして、青い円（"blue"）が新たな空き場所となる。
  3. 待ち行列リストの先頭の情報を確認し、ブロックの色("blue")と空き場所の色("blue")が一致しているため、そのブロックがある場所("undef")へ移動し、空き場所まで運ぶ。この場所は設置が完了したので設置未完了リストから削除する。そして、黒い円（"undef"）が新たな空き場所となる。
  4. 待ち行列リストの先頭の情報を確認し、ブロックの色("yellow")と空き場所の色("undef")が一致しないため、ブロックの色と同じ場所("yellow")へ移動し、そこにあるブロック（"green"）を空き場所("undef")へ運ぶ。運んだブロックの色と空き場所が一致していないため、待ち行列に情報("green", "undef")を追加する。そして、黄色い円（"yellow"）が新たな空き場所となる。
  5. 待ち行列リストの先頭の情報を確認し、ブロックの色("yellow")と空き場所の色("yellow")が一致しているため、そのブロックがある場所("red")へ移動し、空き場所まで運ぶ。この場所は設置が完了したので設置未完了リストから削除する。そして、赤い円（"red"）が新たな空き場所となる。
  6. 待ち行列リストの先頭の情報を確認し、ブロックの色("green")と空き場所の色("red")が一致しないため、ブロックの色と同じ場所("green")へ移動し、そこにあるブロック（"red"）を空き場所("undef")へ運ぶ。運んだブロックの色と空き場所が一致しているため、設置未完了リストから削除する。そして、緑色の円（"green"）が新たな空き場所となる。
  7. 待ち行列リストの先頭の情報を確認し、ブロックの色("green")と空き場所の色("green")が一致しているため、そのブロックがある場所("undef")へ移動し、空き場所まで運ぶ。この場所は設置が完了したので設置未完了リストから削除する。そして、黒い円（"undef"）が新たな空き場所となる。
  8. 設置未完了のリストが空になったため、作業を終える。

4. 2 プログラムの作成例

検証ができたので、考えたアルゴリズムに沿ってプログラムを作成していきます。

■ 変数の定義

main()関数の始めに、情報の管理に使う、3つの変数を定義します。また、一連の作業はボタンAが押されたら開始するようにしておきましょう。

変更・追加【87行目～91行目】
def main():
    from pystubit.board import button_a, display

    robotarm = RobotArm(pin_arm="P13", pin_hand="P14", pin_body="M1", pin_irp="P0", pin_cs="I2C")

    while True:
        if button_a.is_pressed():  # ボタンAが押されたら作業を開始
            incomplete = ["red", "green", "blue", "yellow"]  # 設置未完了のリストを作成
            free = "undef"  # 空き場所の定義
            queue = []  # 運搬待ちのブロックと場所のリストを作成

■ 移動先の選択

次に、ロボットアームの移動先を決定します。もし、運搬待ちのリスト（queue）が空でない場合は、そのリストから情報を取り、移動先（変数target）を決定します。反対に運搬待ちリストが空の場合は、設置未完了のリストの先頭を移動先として選択します。また、選択した移動先に置かれているブロックの色が分かっている場合は、変数b_colorにその情報を格納し、分かっていない場合はNoneを代わりに格納します。

※ リストqueueには、(ブロックの色, 置かれている場所の色)のタプルとしてデータが格納されています。
追加【94行目～106行目】
def main():
    from pystubit.board import button_a, display

    robotarm = RobotArm(pin_arm="P13", pin_hand="P14", pin_body="M1", pin_irp="P0", pin_cs="I2C")

    while True:
        if button_a.is_pressed():
            incomplete = ["red", "green", "blue", "yellow"]
            free = "undef"
            queue = []

            while len(incomplete):  # 未完了リストが空になるまで作業を繰り返す
                if len(queue):  # 運搬待ちのリストにデータがある場合は優先する
                    if queue[0][0] is free:  # 運搬待ちのブロックと同じ色の場所が空いている場合
                        data = queue.pop(0)  # 設置を完了できるため、待ち行列からデータを削除
                        b_color = data[0]  # 運搬待ちのブロックの色情報を格納
                        target = data[1]  # そのブロックが置かれている場所へ移動
                    else:  # 運搬待ちのブロックと同じ色の場所が空いていない場合
                        # 設置は完了できないため、データは待ち行列に残したままにする
                        target = queue[0][0]  # 運搬待ちのブロックと同じ色の場所へ移動
                        b_color = None  # 移動先にあるブロックの色は分かっていない
                else:  # 運搬待ちのリストにデータがない場合
                    target = incomplete[0]  # 設置未完了のリストの先頭を移動先にする
                    b_color = None  # 移動先にあるブロックの色は分かっていない

■ ブロックを持ち上げて色を識別する

今度は、選択した移動先まで回転し、ブロックを持ち上げます。このとき、ブロックの色が分かっていない場合は、色を識別して、LEDディスプレイに色名をスクロール表示します。

追加【107行目～111行目】
            while len(incomplete):
                if len(queue):
                    if queue[0][0] is free:
                        data = queue.pop(0)
                        b_color = data[0]
                        target = data[1]
                    else:
                        target = queue[0][0]
                        b_color = None
                else:
                    target = incomplete[0]
                    b_color = None
                 # ロボットアームを移動し、ブロックを持ち上げる
                robotarm.move_to_color(target)
                robotarm.pick()
                if b_color is None:  # 持ち上げたブロックの色が分かっていない場合
                    b_color = robotarm.recognize_color()  # 色を識別する
                    display.scroll(b_color, delay=10)  # 識別した色の名前をスクロール表示

■ 持ち上げたブロックの色と移動先の場所の色が一致していた場合は設置を完了する

持ち上げたブロックの色と移動先の場所の色が一致していた場合は、そのままブロックを置き直します。そして、その場所は設置が完了しているので、設置未完了リスト（incomplete）から削除して、次のループに移ります。

追加【112行目～115行目】
                robotarm.move_to_color(target)
                robotarm.pick()
                if b_color is None:
                    b_color = robotarm.recognize_color()
                    display.scroll(b_color, delay=10)

                    if b_color == target:  # 持ち上げたブロックの色と移動した場所の色が同じ場合
                        robotarm.place()  # そのままブロックを置き直す
                        incomplete.remove(target)  # その場所は設置が完了しているので、未完了リストから削除
                        continue  # 次のループに移る

■ 空いている場所へブロックを運ぶ

次のループへ移らない場合は、空いている場所（free）へブロックを運びます。このとき、その空き場所とブロックの色が一致していれば、設置完了として、未完了リストからその場所を削除します。そうでない場合は、ブロックの色と置いた場所をタプルにまとめ、運搬待ちリストに追加します。そして最後に、元々ブロックがあった場所（target）が空になったので、変数freeに格納します。

追加【118行目、119行目、122行目～126行目】
                robotarm.move_to_color(target)
                robotarm.pick()
                if b_color is None:
                    b_color = robotarm.recognize_color()
                    display.scroll(b_color, delay=10)

                    if b_color == target:
                        robotarm.place()
                        incomplete.remove(target)
                        continue

                # 空いている場所へブロックを運搬する
                robotarm.move_to_color(free)
                robotarm.place()

                # 空き場所と運搬したブロックの色が一致する場合は、設置が完了しているので未完了リストから削除
                if b_color is free:
                    incomplete.remove(free)
                else:  # そうでない場合は、運搬したブロックとそれを置いた場所を待ち行列に追加
                    queue.append((b_color, free))
                free = target  # ブロックの運搬元が新たな空き場所となっているので変更する

■ 始めの位置に戻る

すべてのブロックの設置が完了できると、93行目のwhile文を抜けます。抜けた後は、最初の位置（１番）に移動するようにしましょう。

追加【127行目】
                if b_color is free:
                    incomplete.remove(free)
                else:
                     queue.append((b_color, free))
                free = target

            robotarm.move_to(1)  # すべてのブロックの設置を完了した後は１番へ戻る

4. 3 プログラムを実行して動作を確認する

それでは、完成したプログラムを実行します。様々な置き方を試し、どのような場合でも対応できていることを確認しましょう。

【 サンプルコード 5-2-1 】
def main():
    from pystubit.board import button_a, display

    robotarm = RobotArm(pin_arm="P13", pin_hand="P14", pin_body="M1", pin_irp="P0", pin_cs="I2C")

    while True:
        if button_a.is_pressed():  # ボタンAが押されたら処理を開始
            incomplete = ["red", "green", "blue", "yellow"]  # 未完了リストの作成
            free = "undef"  # 空きスペースの定義
            queue = []  # 運搬待ちのブロックと場所のリスト

            while len(incomplete):  # 未完了リストが空になるまで作業を繰り返す
                if len(queue):  # 運搬待ちのリストにデータがある場合は優先する
                    if queue[0][0] is free:  # 運搬待ちのブロックと同じ色の場所が空いている場合
                        data = queue.pop(0)  # 設置を完了できるため、待ち行列からデータを削除
                        b_color = data[0]  # 運搬待ちのブロックの色情報を格納
                        target = data[1]  # そのブロックが置かれている場所へ移動
                    else:  # 運搬待ちのブロックと同じ色の場所が空いていない場合
                        # 設置は完了できないため、データは待ち行列に残したままにする
                        target = queue[0][0]  # 運搬待ちのブロックと同じ色の場所へ移動
                        b_color = None  # 移動先にあるブロックの色は分かっていない
                else:  # 運搬待ちのリストにデータがない場合
                    target = incomplete[0]  # 設置未完了のリストの先頭を移動先にする
                    b_color = None  # 移動先にあるブロックの色は分かっていない

                # ロボットアームを移動し、ブロックを持ち上げる
                robotarm.move_to_color(target)
                robotarm.pick()
                if b_color is None:  # 持ち上げたブロックの色が分かっていない場合
                    b_color = robotarm.recognize_color()
                    display.scroll(b_color, delay=10)

                    # 持ち上げたブロックの色と移動した場所の色が同じ場合は、その場に置き直す
                    if b_color == target:
                        robotarm.place()
                        incomplete.remove(target)  # その場所は設置が完了しているので、未完了リストから削除
                        continue  # 次のループに移る

                # 空きスペースへ運搬する
                robotarm.move_to_color(free)
                robotarm.place()

                # 空きスペースと運搬したブロックの色が一致する場合は、設置が完了しているので未完了リストから削除
                if b_color is free:
                    incomplete.remove(free)
                else:  # そうでない場合は、運搬したブロックとそれを置いた場所を待ち行列に追加
                    queue.append((b_color, free))

                # ブロックの運搬元が新たな空き場所となっているので変更する
                free = target

            robotarm.move_to(1)  # すべてのブロックの設置を完了した後はスタート地点へ戻る

main()  # メイン関数の実行

チャプター5

おわりに

5. 1 このレッスンのまとめ

このレッスンでは、カラーセンサーを使い、色で運搬物の識別を行いましたが、最新の技術ではカメラを使用した画像認識により、運搬物の色だけでなく、輪郭や向きなども認識してロボットアームが制御できるようになっています。

また、センサーを用いて情報を得るだけでなく、そこからどのようにロボットアームを動作させるのかを検討し、適切な制御アルゴリズムを組むことも大切です。ぜひ、今回製作したロボットアームを使用して、自分で立てた課題を達成する制御アルゴリズムの作成に挑戦してみてください。

5. 2 次のレッスンについて

みなさんはここまで、Pythonでロボットアームの制御コードを書いてきましたが、実際にはロボットアームを使用する人の全員が必ずしもプログラミング言語を習得しているとは限りません。そこで、次のレッスンでは簡単なコマンドを入力するだけで、誰でもロボットアームを制御できるようなシステムの開発に取り組みます。

次のレッスンに進む
目次に戻る
TOP