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

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

テーマ.8-2 赤外線フォトリフレクタを使用した位置検出

センサーを使用して位置検出をしよう！

チャプター1

このレッスンで学ぶこと

このレッスンでは、レッスン30で製作したロボットアームに赤外線フォトリフレクタを追加して、フィールド上の位置を検出して移動する機能を作成します。

チャプター2

赤外線フォトリフレクタの追加

ボディの先端に新たに赤外線フォトリフレクタを取り付けます。この赤外線フォトリフレクタでフィールドに描かれた黒線を読み取ることで、位置検出を行います。

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

【 パーツ一覧 】
  • レッスン30で製作したロボットアーム×1
  • 赤外線フォトリフレクタ×1
  • センサー接続コード（3芯15cm）×1
  • ブロックハーフC（白）×6
  • ブロックハーフD（白）×2
【 アーテックブロックの形状 】

2. 2 組立説明書

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

赤外線フォトリフレフレクタの取り付け方

チャプター3

フィールド上の位置を指定してボディを回転するプログラムの作成

キットに付属しているフィールドには、６つの箇所に色のちがう小さな円が描かれています。これらの円に対して、それぞれ時計回りに次のように番号を設定します。

前回のレッスンでは、ボディのDCモーターの「回転時間」を微調整することで、特定の位置へ移動させてブロックを運ぶ課題に取り組みました。しかし、DCモーターはプログラム上で一定の速さに設定にしていても、電池の残量で実際の速さが変わってしまいます。そのため、上記のやり方だと時間が経つにつれて移動先の位置が少しずつずれていってしまいます。

ここで、赤外線フォトリフレクタを使うと「黒線とそれ以外（空白部分）を識別できた」ことを思い出してください。フィールドに描かれた黒線を検出しながらボディを回転できれば、モーターの速さに関係なく、いつでも正確に決まった位置まで移動させることができます。

3. 1 黒線を検出するためのしきい値の設定

では、最初に黒線を検出するための「しきい値」を設定します。ここでは、より正確に黒線を検出できるように、黒線寄りのしきい値threshold_lowと空白寄りのしきい値threshold_highの2つを設定します。

■ 赤外線フォトリフレクタの値を調べる

まずは、以下のコードを実行して、フィールドの黒線上と空白上での赤外線フォトリフレクタの値を調べましょう。

【 1秒おきに赤外線フォトリフレクタの値をターミナルに表示するプログラム 】
from pyatcrobo2.parts import IRPhotoReflector
import time

irp = IRPhotoReflector("P0")

while True:
    val = irp.get_value()
    print(val)
    time.sleep_ms(1000)

■ しきい値を決める（※重要！！）

調べた結果からそれぞれのしきい値を決めます。およその目安として、調べた値と300ほどの差でしきい値を決めると黒線の検出が上手くいきます。

※ 赤外線フォトリフレクタの値は、個体によってばらつきがあるため上記の設定が適当ではないケースもあります。後で作成するプログラムで上手く動作しない場合は、しきい値を調整してください。
【 しきい値の例 】

■ RobotArmクラスのプロパティとしてしきい値を追加

では、前のレッスンで作成したRobotArmクラスへ、このしきい値をプロパティとして追加しましょう。前のプログラムを削除してしまった場合は、以下のコードをそのままコピーして、しきい値のみ書き換えてください。

追加【6行目、7行目】
class RobotArm:
    from pyatcrobo2.parts import DCMotor
    import myservo
    import time

    threshold_high = 900  # 空白寄りのしきい値
    threshold_low = 650  # 黒線寄りのしきい値

    def __init__(self, *, pin_arm, pin_hand, pin_body):
        self.arm = self.myservo.MyServomotor(pin_arm)
        self.hand = self.myservo.MyServomotor(pin_hand)
        self.body = self.DCMotor(pin_body)

    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()

3. 2 指定された番号の位置へ移動するメソッドの追加

今度は、設定したしきい値を使って、「フィールド上の番号を指定するだけで、その場所までボディが回転して移動する」メソッドをmove_to()という名前で追加しましょう。

■ 赤外線フォトリフレクタのインスタンスの追加

まずは、__init__()メソッドで新たな引数として赤外線フォトリフレクタの接続先の端子名を受け取るように変更します。そして、irpプロパティを定義して、IRPhotoReflectorクラスのインスタンスを作成して格納しましょう。

変更・追加【2行目、9行目、13行目】
class RobotArm:
    from pyatcrobo2.parts import DCMotor, IRPhotoReflector  # IRPhotoReflectorクラスのインポート
    import myservo
    import time

    threshold_high = 900
    threshold_low = 650

    def __init__(self, *, pin_arm, pin_hand, pin_body, pin_irp):  # 引数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)  # IRPhotoReflectorクラスのインスタンスを作成

■ 初期位置の設定

赤外線フォトリフレクタで検出できるのは黒線のみです。最初にロボットアームがフィールドに配置されたときに、どの番号の位置にあるのかは調べることができません。そのため、「１」番を初期位置として決め、その黒線の真上に赤外線フォトリフレクタが来るように配置します。

そして、ロボットアームには初期位置を記憶させておきます。positionプロパティを定義して、「1」を格納しましょう。

追加【14行目】
    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  # 初期位置を格納

■ move_to()メソッドの定義

移動を行うmove_to()メソッドでは、引数として番号numを受け取ります。この移動先の番号numと現在の位置を表す番号positionの差から「黒線何本分回転するのか」と「どちらの向きに回転するのか」を決めて、ボディを制御します。

まずは、番号の差（diff）を求めます。そして、その差が「正」であれば回転方向（direction）を時計回り（"cw"）に、差が「負」であれば反時計回り（"ccw"）としましょう。

追加【39行目～41行目】
    def stop(self):
        self.body.brake()

    def move_to(self, num):
        diff = num - self.position  # 移動先の番号と現在の位置の番号の差を求める
        direction = "cw" if diff > 0 else "ccw"  # 差が「正」であれば時計回り、「負」であれば反時計回り
※ diffは英語で「差」の意味を表す「difference（ディファレンス）」を省略した表記です。

次に、求めた「回転方向」でボディを回転します。時間を指定して制御するわけではないため、引数durationは省略します。

追加【42行目】
    def move_to(self, num):
        diff = num - self.position
        direction = "cw" if diff > 0 else "ccw"
        self.rotate(direction=direction)  # 引数「speed」も省略可能

では、時間の代わりに何によって移動量を決めるのでしょうか？ようやくここで、赤外線フォトリフレクタの出番です。

今いる黒線から次の黒線まで移動するときの赤外線フォトリフレクタの値の変化を考えてみましょう。設定した2つのしきい値を使って説明すると次の変化が起きています。

そのため、制御としては「センサー値 > threshold_high まで待つ（空白上に移動するまで待つ）」と「センサー値 < threshold_highまで待つ（黒線上に移動するまで待つ）」を順番に行うと、次の黒線に移動することになります。

「まで待つ」というPythonの構文はありませんが、以下のようにwhile文とnot演算子を組み合わせることで実現できます。

while not センサー値 > threshold_high:  # 空白上に移動するまで待つ
    pass
while not センサー値 < threshold_low:  # 黒線上に移動するまで待つ
    pass

これを番号の差の分だけfor文で繰り返し、最後に回転を停止することで移動完了になります。このとき１つ注意が必要で、差は「負」の場合もあるため、あらかじめ標準関数のabsで絶対値に変換する必要があります。

追加【43行目～48行目】
    def move_to(self, num):        diff = num - self.position        direction = "cw" if diff > 0 else "ccw"        self.rotate(direction=direction)        for _ in range(abs(diff)):  # abs()関数で差を絶対値に変換することに注意            while not self.irp.get_value() > self.threshold_high:  # 空白上に移動するまで待つ                pass            while not self.irp.get_value() < self.threshold_low:  # 黒線上に移動するまで待つ                pass        self.stop()  # ボディの回転を停止

最後に移動した先の番号を、positionプロパティに記録します。これで、この新しい番号が次に移動するときの基準となります。

追加【49行目】
    def move_to(self, num):
        diff = num - self.position
        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  # 移動先の番号を記録

3. 3 動作の確認

完成したRobotArmクラスの定義は以下になります。

【 サンプルコード 3-2-1 】
class RobotArm:
    from pyatcrobo2.parts import DCMotor, IRPhotoReflector
    import myservo
    import time

    threshold_high = 900
    threshold_low = 650

    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
        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

このクラスを使って、3つのブロックを指定された位置へ順番に運ぶテストプログラムをつくり、動作を確認しましょう。

【 テストプログラムの例 】
robotarm = RobotArm(pin_arm="P13", pin_hand="P14", pin_body="M1", pin_irp="P0")

robotarm.pick()  # 白黒のブロックを4番（黒の円）に運ぶ
robotarm.move_to(4)
robotarm.place()

robotarm.move_to(2)  # 赤緑のブロックを3番（緑の円）に運ぶ
robotarm.pick()
robotarm.move_to(3)
robotarm.place()

robotarm.move_to(6)  # 黄青のブロックを5番（青の円）に運ぶ
robotarm.pick()
robotarm.move_to(5)
robotarm.place()

robotarm.move_to(1)  # 初期位置の1番に戻る

チャプター4

課題：最短距離でブロックを運べるように改良しよう

前のチャプターで新たにRobotArmクラスに追加したmove_to()メソッドでは、場合によっては効率の悪い移動を行うことがあります。

例えば、１番のブロックを６番へ運ぶときは、時計回りに大きく回りますが、実際は反時計回りに回転した方が効率が良いわけです。

そこで、この課題では移動距離が短くなる方向へボディが回転して、ブロックを運べるようにプログラムを改良してみましょう。

4. 1 プログラムの考え方

実は、理論が分かればこのプログラムの変更はとても簡単です。具体的には、反対向きに回転させたい場合は、元々の向きに「－６」や「＋６」をすることで変換できます。そのため、求めた番号の差が「正」の場合と「負」の場合に分けて、次の式で差の変換を行います。

  • 差が「正」の場合
変換後の差　＝　求めた番号の差 － 6
  • 差が「負」の場合
変換後の差　＝　求めた番号の差 ＋ 6

具体的な例で本当にこの変換式が正しいのか検証してみましょう。

問題なく変換できていることがわかります。そこで、差の絶対値が3より大きい場合は、反対向きにボディ回転した方が効率が良いため、上で確認した変換式を使いましょう。

4. 2 プログラムの変更例

上で確認した変換の手順をコードで書くと次のようになります。自分の書いたコードが上手くいかなかったときは、こちらを参考にして修正しましょう。

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

    threshold_high = 900
    threshold_low = 650

    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:  #　差の絶対が3より大きい場合は反対向きに回転した方が効率が良い
            diff = diff - 6 if diff > 0 else diff + 6  # 差が正の場合と負の場合で変換式を分ける
        direction = "cw" if diff > 0 else "ccw"  # ここから下は元のコードのままでOK
        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

robotarm = RobotArm(pin_arm="P13", pin_hand="P14", pin_body="M1", pin_irp="P0")
 
robotarm.pick()
robotarm.move_to(4)
robotarm.place()
 
robotarm.move_to(2)
robotarm.pick()
robotarm.move_to(3)
robotarm.place()
 
robotarm.move_to(6)
robotarm.pick()
robotarm.move_to(5)
robotarm.place()
 
robotarm.move_to(1)

チャプター5

おわりに

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

このレッスンでは、前のレッスンで製作したロボットアームへ位置を検出する機能を追加しました。正確な動作が求められるロボットにとって、センサーを使った位置検出機能は必須です。今回は簡単な仕組みで位置検出を行いましたが、実際のロボットでは、複数のセンサーから取得したデータを使い内部で複雑な計算処理を実行することで、正確な制御を実現しています。これを理解するには、物理や数学の知識が必要になりますので、今は学校でしっかりとそれらの教科を学ぶようにしましょう。

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

次のレッスンでは、新たにカラーセンサーをロボットアームへ追加し、運搬物を色で識別して運ぶ機能を開発します。

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