Pythonロボティクスコース レッスン 33
テーマ.8-4 ロボットアームの制御用システムの開発
誰でも簡単にロボットアームを制御できるシステムを開発しよう！
チャプター1
このレッスンで学ぶこと
製品として販売されているロボットアームには、一般的なプログラミング言語を習得していない人でもロボットアームへの指示が出せるような、制御用ソフトウェアが合わせて提供されています。
そこで、このレッスンでは製作したロボットアームをターミナル上で簡単なコマンドを入力するだけで、誰でも簡単に制御できるシステムの開発に取り組みます。
チャプター2
新しいPython文法の学習
ロボットアームの製作に入る前に、新たなPythonの文法として「集合」について学習しましょう。
2. 1 集合について知ろう
集合は、リストや辞書、タプルのように複数の値を持つデータ型です。「set型」とも呼ばれ、集合のデータを定義する場合は、次のように波括弧{}でデータを囲みます。
my_set = {1, 2, 3}
ではまず、集合のデータ構造から確認していきましょう。
■ 集合のデータ構造
集合では、リストや辞書などで使用していたインデックスやキーは存在せず、要素に順序がありません。また、集合に含まれるデータは重複がなく、全てユニークな（集合内にひとつしかない）データとなります。
my_set_OK = {1, 2, 3, 4, 5} # ○ 重複する数字がない
my_set_NG = {1, 2, 2, 3, 3} # × 重複する数字がある
リストや辞書のデータ構造は、各データが１つずつ箱に格納されているイメージでよく説明されます。一方で集合のデータ構造は、複数のデータが１つの大きな袋にまとまって入っている様子をイメージしてください。
■ 集合で使用するメソッド
集合には、次の3つのメソッドが用意されています。
| メソッド名 | 内容 |
|---|---|
add() | 要素の追加する。既にその要素がある場合は無視されるが、エラーにはならない。 |
remove() | 指定した要素を削除する。 |
clear() | すべての要素を削除する。 |
以上の3つのメソッドの使い方について、下のサンプルコードで見ていきましょう。
【サンプルコード 2-1-1 】
# 集合(set)型の定義
my_set = {1,2,3}
print("集合(set)型の定義")
print(my_set)
# add()メソッドを使用して要素の追加
my_set.add(4)
print("add()メソッドを使用して要素に「4」を追加")
print(my_set)
# add()メソッドを使用して要素の追加
my_set.add(3)
print("add()メソッドを使用して要素に「3」を追加")
print(my_set)
# remove()メソッドを使用して指定した要素の削除
my_set.remove(3)
print("remove()メソッドを使用して「3」の要素を削除")
print(my_set)
# clear()メソッドを使用して全ての要素の削除
my_set.clear()
print("clear()メソッドを使用して全ての要素の削除")
print(my_set)
（実行結果）
集合(set)型の定義
{3, 1, 2}
add()メソッドを使用して要素に「4」を追加
{4, 1, 2, 3}
add()メソッドを使用して要素に「3」を追加
{4, 1, 2, 3}
remove()メソッドを使用して「3」の要素を削除
{4, 1, 2}
clear()メソッドを使用して全ての要素の削除
set()12行目のadd()メソッドで、「3」を追加したところに注目してください。「3」はすでに要素として持っているため、実行結果を見ると反映されていません。このように、集合では追加する要素が重複している場合、その処理は無視されます。
2. 2 集合の演算について知ろう
集合は数学の世界でも使われている概念で、「いくつかのものからなる集まり」を表しています。また、集合どうしは数字と同じく足し合わせたり、引いたりすることができ、「和」「差」「積」の基本となる３つの演算があります。
例として、次の２つの「数字」の集まりを取り挙げて、集合の演算について見ていきましょう。
■ 和の演算
AとBの和の演算を行うと、2つの要素を合わせた集まりとなり、このような集合のことをAとBの「和集合」といいます。和集合では、2つの集合の要素に重なりがある場合はひとつにまとめられます。
Pythonの集合でも、それぞれの演算に対応した演算子が用意されています。和集合の演算では、「|（傍線：ぼうせん）」の記号を演算子として使います。
【 サンプルコード 2-2-1】
# 集合A、集合Bの定義
A = {1, 2, 3, 5}
B = {3 ,4, 5, 6, 7}
# 集合Aと集合Bの和集合の演算
AB = A | B
print(AB)
（実行結果）
{1, 2, 3, 4, 5, 6, 7}■ 差の演算
AとBの差の演算を行うと、AからBに含まれる要素を取り除いた集まりとなり、このような集合のことをAとBの「差集合」といいます。
Pythonでは差集合の演算に「－」の記号を使います。
【 サンプルコード 2-2-2】
# 集合A、集合Bの定義
A = {1, 2, 3, 5}
B = {3 ,4, 5, 6, 7}
# 集合Aと集合Bの差集合の演算
AB = A - B
print(AB)
（実行結果）
{1, 2}積の演算
AとBの積の演算を行うと、AとBの両方に含まれる要素の集まりとなり、このような集合のことをAとBの「積集合」といいます。
Pythonでは積集合の演算に「&」の記号を使います。
【 サンプルコード 2-2-3】
# 集合A、集合Bの定義
A = {1, 2, 3, 5}
B = {3 ,4, 5, 6, 7}
# 集合Aと集合Bの積集合の演算
AB = A & B
print(AB)
（実行結果）
{3, 5}排他的論理和の演算
和・差・積の他によく使われる演算として「排他的論理和（はいたてきろんりわ）」があります。言葉としては少し難しいのですが、図で考えるとシンプルで、AとBの2つの要素を合わせた集まりから、両方に含まれる要素を取り除いた集まりとなります。
Pythonでは排他的論理和集合の演算に「^（ハットマーク）」の記号を使います。
【 サンプルコード 2-2-4】
# 集合A、集合Bの定義
A = {1, 2, 3, 5}
B = {3 ,4, 5, 6, 7}
# 集合Aと集合Bの排他的論理和集合の演算
AB = A ^ B
print(AB)
（実行結果）
{6, 1, 2, 7, 4}2. 3 リスト型のデータから集合型のデータを作成しよう
集合のデータはリストのデータから作成することもできます。それには、set()関数を使用します。
以下のサンプルコードでは、重複した数字を要素にもつリストmylistから、集合のデータmysetを作成しています。
【 サンプルコード 2-3-1 】
# リスト型データの定義
mylist = [1, 2, 2, 3, 3, 3, 4, 5, 6, 7]
# set()関数を使用してリスト型データから集合型データの作成
myset = set(mylist)
print(myset)
【実行結果】
{1, 2, 3, 4, 5, 6, 7}
上の例では、[1, 2, 2, 3, 3, 3, 4, 5, 6, 7]と重複した要素を持つリストを用意しました。しかし、集合に変換すると、{1, 2, 3, 4, 5, 6, 7}と重複のない結果となっています。このように、リストから集合へ変換すると、自動的に重複した要素が取り除かれるようになっています。
2. 4 集合を使用するメリット
ここまでで確認してきたように、集合には次の2つの大きな特徴があります。
- 要素の重複がない。
- 集合演算が使用できる。
「1」の特長からは、例えば「会員番号」や「電話番号」など、重複がないデータを管理するときに適していることが分かります。反対に「人の名前」や「生年月日」など重複するデータの管理には向いていません。また、重複のあるリストのデータから重複する要素を削除して、データを整理するときにも使用できます。
「2」の特長を活かすと、例えば「Aの特徴を持つ動物」「Bの特徴を持つ動物」「Cの特徴を持つ動物」のようにカテゴリ分けをした中から、「AとBとCのどの特徴も持つ動物」を抽出することだって簡単にできます。
animals = A & B & C
これまでリストや辞書で書いていたコードも、実は集合を使うことで、より短くまとめられるかもしれません。それぞれのデータ型の特徴を正しく理解して、洗練されたコードを書けるようになりましょう。
チャプター3
コマンドを入力してロボットアームを操作する
ロボットアームを利用する人達は、必ずしもプログラミングへの理解がある人ばかりではありません。そのため、ロボットアームを手動で動かすだけで指示ができたり、Pythonのように汎用的なプログラミング言語ではなく、ロボットの制御に特化して簡易化されたプログラム言語（通称：ロボット言語）が用意されていたりします。
そこで、ここからはターミナルへ以下の簡単なコマンド文を入力するだけで、Pythonを知らない人でも簡単にロボットアームが制御できるように、前のレッスンで作成したRobotArmクラスを利用してシステムを作成します。
【 ロボットアームへ動作を命令するときのコマンド文 】
|コマンド文|対応する動作|
|:—|:—|
|pick|その場でつかんで持ち上げる|
|place|その場につかんでいるものを置く|
|move A|ボディを回転して、指定した位置（A）へ移動する|
|transport A B|指定した位置（A）から別の位置（B）へものを運ぶ|
コマンド文の「A」や「B」には、レッスン30から使用しているフィールド上の位置を表す番号かもしくはその色名を指定します。そのため、ターミナルへは次のようにコマンドを入力します。
【 コマンドの入力例 】
--> move 1 # 1の位置へ移動 --> pick # つかんで持ち上げる --> move 2 # 2の位置へ移動 --> lift # つかんでいるものを置く --> transport green blue # 緑の円にあるものを青の円へ運ぶ
それぞれの動作のコマンドはRobotArmクラスの以下のメソッドに対応します。
- pick
RobotArm.pick()
- place
RobotArm.place()
- move A
RobotArm.move_to(num) または RobotArm.move_to_color(color)
- transport A B
RobotArm.move_to(num) または RobotArm.move_to_color(color) RobotArm.pick() RobotArm.move_to(num) または RobotArm.move_to_color(color) RobotArm.place()
では、これらのコマンドを使用してロボットアームを操作できるように、プログラムを作成していきましょう。
■ RobotArmクラスのモジュール化
まずは、前のレッスンまでで作成したRobotArmクラスをモジュール化します。
プログラムを削除してしまった場合は、以下のコードを複製して新しく作成したファイルに貼り付けましょう。このファイルを「robotarm(.py)」という名前で保存し、Studuino:bit内にも保存しましょう。
【 robotarmモジュール 】
※ 15行目、16行目のしきい値と28行目のつかむ動作を行うときのロボットアームの手の角度は各々調整してください。
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, 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
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)
■ robotarmモジュールのインポートとインスタンスの作成
では早速、新しいプログラムファイルを作成し、先頭で用意したモジュールからRobotArmクラスをインポートします。そして、インスタンスを作成しましょう。
from robotarm import RobotArm
robotarm = RobotArm(pin_arm="P13", pin_hand="P14",
pin_body="M1", pin_irp="P0", pin_cs="I2C")
■ 使用できるコマンド一覧の定義
次に使用可能なコマンドを、種類ごとに集合として定義します。これらは後で、入力されたコマンドに誤りがないかどうかをチェックするために使います。
※ 位置を表す番号も文字列として定義していることに注意してください。これはターミナルから入力されたコマンドがすべて文字列として処理されるため、それに合わせています。
追加【6行目～8行目】
from robotarm import RobotArm
robotarm = RobotArm(pin_arm="P13", pin_hand="P14",
pin_body="M1", pin_irp="P0", pin_cs="I2C")
COMMANDS = {"pick", "place", "move", "transport"} # 動作コマンド
POSITIONS = {"1", "2", "3", "4", "5", "6"} # 位置を表す番号
COLORS = {"grey", "red", "green", "blue", "yellow", "undef"} # 位置を表す色名
■ コマンドの入力を受け付ける
コマンドの入力受付はinput()関数で行います。入力されたコマンドは一旦変数dataに格納し、そこから文字列のsplit()メソッドを使って、半角スペースで要素を区切られたリストに変換します。例えば「transport 1 2」を入力した場合、「["transport", "1", "2"]」に変換されます。
追加【10行目～13行目】
from robotarm import RobotArm
robotarm = RobotArm(pin_arm="P13", pin_hand="P14",
pin_body="M1", pin_irp="P0", pin_cs="I2C")
COMMANDS = {"pick", "place", "move", "transport"}
POSITIONS = {"1", "2", "3", "4", "5", "6"}
COLORS = {"grey", "red", "green", "blue", "yellow", "undef"}
def main(): # 関数の中に処理をまとめる
while True:
data = input("-->") # ターミナルでコマンドの入力を受付ける
command = data.split() # コマンドの文字列をスペースで分割してリスト化
■ 入力されたコマンドに誤りがないか確認をする
入力の誤りは、try-except文で適切に処理します。具体的にはtry文の中で、次の順番で誤りがないを調べていき、誤りが見つかった場合は、メッセージを付けて例外を発生させます。
- 該当する動作コマンドがあるか？
- 該当したコマンドが
moveの場合、引数は１つだけか？ - 2で問題がなかった場合、引数として渡された場所を表す番号や色名に問題はないか？
- 該当したコマンドが
transportの場合、引数は２つだけか？ - 4で問題がなかった場合、引数として渡された場所を表す番号や色名に問題はないか？
この順番に沿って、コードを書くと次のようになります。誤りが見つかった場合は、変数msgにその誤りの内容を入れ、raise文に例外クラスExceptionのインスタンスを渡します。
追加【12行目、15行目～31行目】
def main():
while True:
try: # try文で例外を捕捉
data = input("-->")
command = data.split()
msg = None # メッセージを格納する変数
if not command[0] in COMMANDS: # 該当する動作コマンドがない
msg = "'{}' is an invalid command.".format(command[0])
elif command[0] == "move": # 動作コマンドが"move"の場合
if not len(command) == 2: # 1個の引数を含めて要素数が2つ以外の場合はエラー
msg = "'move' only needs 1 argument."
elif not (command[1] in POSITIONS or command[1] in COLORS): # 引数に誤りがある場合もエラー
msg = "'{}' is an invalid argument.".format(command[1])
elif command[0] == "transport":
if not len(command) == 3: # 2個の引数を含めて要素数が3つ以外の場合はエラー
msg = "'transport' only needs 2 arguments."
elif not (command[1] in POSITIONS or command[1] in COLORS): # ひとつめの引数に誤りがある場合もエラー
msg = "'{}' is an invalid argument.".format(command[1])
elif not (command[2] in POSITIONS or command[2] in COLORS): # ふたつめの引数に誤りがある場合もエラー
msg = "'{}' is an invalid argument.".format(command[2])
if msg: # メッセージがある場合は例外を発生させる
raise Exception(msg) # メッセージがNoneのままなら例外は発生しない
※ 「invalid」は、英語で「無効な」の意味を表します。
※ 「argument」は、英語で「引数」の意味を表します。
■ 誤りが検出されたときにエラー内容を表示する
raise文で挙げた例外をexcept文で捕捉します。また、補足した例外に記録されている誤りの内容をprint()関数で表示します。この場合はロボットアームを制御することはできないので、continue文で次のループに移ります。
追加【32行目～34行目】
def main():
while True:
try:
data = input("-->")
command = data.split()
msg = None
if not command[0] in COMMANDS:
msg = "'{}' is an invalid command.".format(command[0])
elif command[0] == "move":
if not len(command) == 2:
msg = "'move' only needs 1 argument."
elif not (command[1] in POSITIONS or command[1] in COLORS):
msg = "'{}' is an invalid argument.".format(command[1])
elif command[0] == "transport":
if not len(command) == 3:
msg = "'transport' only needs 2 arguments."
elif not (command[1] in POSITIONS or command[1] in COLORS):
msg = "'{}' is an invalid argument.".format(command[1])
elif not (command[2] in POSITIONS or command[2] in COLORS):
msg = "'{}' is an invalid argument.".format(command[2])
if msg:
raise Exception(msg)
except Exception as e: # 例外を捕捉
print(e) # エラー内容を表示
continue # 次のループに移る
■ コマンドに応じてロボットアームを制御する
入力されたコマンドに誤りがない、つまり例外が捕捉されない場合は、コマンドに従ってロボットアームを制御します。RobotArmクラスのmove_to()メソッドの引数の型が整数（int）である点に注意して、以下のようにコードを書きましょう。
追加【35行目～55行目】
except Exception as e:
print(e)
continue
else: # 入力されたコマンドに誤りがない（例外が捕捉されない）場合
if command[0] == "pick":
robotarm.pick()
elif command[0] == "place":
robotarm.place()
elif command[0] == "move":
if command[1] in POSITIONS: # 位置が番号で指定されている場合
robotarm.move_to(int(command[1])) # 文字列を数値に変換する必要あり
else: # 色名で位置が指定されている場合
robotarm.move_to_color(command[1])
else: # コマンドが"transport"の場合
if command[1] in POSITIONS: # 位置が番号で指定されている場合
robotarm.move_to(int(command[1]))
else: # 色名で位置が指定されている場合
robotarm.move_to_color(command[1])
robotarm.pick()
if command[2] in POSITIONS: # 位置が番号で指定されている場合
robotarm.move_to(int(command[2]))
else: # 色名で位置が指定されている場合
robotarm.move_to_color(command[2])
robotarm.place()
■ 完成したプログラムの動作を確認する
以上でプログラムの完成です。プログラムの実行後、ターミナルからmain()関数を実行し、確認した4つのコマンドを順番に入力してみましょう。
※ moveやtransportの動作を確認するときは、前回のレッスンでも使用したロボットアーム用フィールドが必要です。
※ また、事前にフィールドの1番の位置にブロックを置き、ロボットアームを合わせましょう。
【 コマンドの入力例 】
>>> main() -->pick -->move 2 -->place -->transport 2 3
完成したプログラムは以下の通りです。うまくいかない場合は自分の書いたコードと見比べましょう。
【 サンプルコード 3-1-1 】
from robotarm import RobotArm
robotarm = RobotArm(pin_arm="P13", pin_hand="P14",
pin_body="M1", pin_irp="P0", pin_cs="I2C")
COMMANDS = {"pick", "place", "move", "transport"}
POSITIONS = {"1", "2", "3", "4", "5", "6"}
COLORS = {"grey", "red", "green", "blue", "yellow", "undef"}
def main():
while True:
try:
data = input("-->")
command = data.split()
msg = None
if not command[0] in COMMANDS:
msg = "'{}' is an invalid command.".format(command[0])
elif command[0] == "move":
if not len(command) == 2:
msg = "'move' only needs 1 argument."
elif not (command[1] in POSITIONS or command[1] in COLORS):
msg = "'{}' is an invalid argument.".format(command[1])
elif command[0] == "transport":
if not len(command) == 3:
msg = "'transport' only needs 2 arguments."
elif not (command[1] in POSITIONS or command[1] in COLORS):
msg = "'{}' is an invalid argument.".format(command[1])
elif not (command[2] in POSITIONS or command[2] in COLORS):
msg = "'{}' is an invalid argument.".format(command[2])
if msg:
raise Exception(msg)
except Exception as e:
print(e)
continue
else:
if command[0] == "pick":
robotarm.pick()
elif command[0] == "place":
robotarm.place()
elif command[0] == "move":
if command[1] in POSITIONS:
robotarm.move_to(int(command[1]))
else:
robotarm.move_to_color(command[1])
else:
if command[1] in POSITIONS:
robotarm.move_to(int(command[1]))
else:
robotarm.move_to_color(command[1])
robotarm.pick()
if command[2] in POSITIONS:
robotarm.move_to(int(command[2]))
else:
robotarm.move_to_color(command[2])
robotarm.place()
チャプター4
課題：識別した色によって動作を変えるコマンドの追加
この課題では、カラーセンサーを使用した色の識別によって行う動作を変えるコマンドを新たに追加します。
【 追加するコマンド 】
| コマンド文 | 内容 |
|---|---|
recognize COLOR A | 持ち上げたブロックの色を調べ、指定した色（COLOR）であった場合、指定した位置（A）へ運ぶ。もし、指定した色でなかった場合はその場に置く。 |
このコマンドは次のように使用します。
-->recognize red 3 # 持ち上げたブロックが赤色なら3の位置へ運ぶ
では、【 サンプルコード 3-1-1 】を改造してこのコマンドが使用できるようにしましょう。
4. 1 プログラムの改造例
ひとつの例として、以下のステップでプログラムを改造できます。
■ コマンド一覧に「recognize」を追加する
コマンドの一覧COMMANDSに新たに"recognize"を追加します。
変更【3行目】
from robotarm import RobotArm
COMMANDS = {"pick", "place", "move", "transport", "recognize"} # recognizeを追加
POSITIONS = {"1", "2", "3", "4", "5", "6"}
COLORS = {"grey", "red", "green", "blue", "yellow", "undef"}
■ 例外処理に対応する
recognizeコマンドが使用された場合についても、引数が足りなかったり、引数に指定した名前が不適切だったときは例外を挙げるようにコードを追加します。
追加【30行目～36行目】
def main():
while True:
try:
data = input("-->")
command = data.split()
msg = None
if not command[0] in COMMANDS:
msg = "'{}' is an invalid command.".format(command[0])
elif command[0] == "move":
if not len(command) == 2:
msg = "'move' only needs 1 argument."
if not (command[1] in POSITIONS or command[1] in COLORS):
msg = "'{}' is an invalid argument.".format(command[1])
elif command[0] == "transport":
if not len(command) == 3:
msg = "'transport' only needs 2 arguments."
if not (command[1] in POSITIONS or command[1] in COLORS):
msg = "'{}' is an invalid argument.".format(command[1])
if not (command[2] in POSITIONS or command[2] in COLORS):
msg = "'{}' is an invalid argument.".format(command[2])
elif command[0] == "recognize": # recognizeコマンドのとき
if not len(command) == 3: # 2つの引数がか必ず必要になるためリストの要素数は3になる
msg = "'recognize' only needs 2 arguments."
elif not command[1] in COLORS: # 1つめの引数は色名のみ
msg = "'{}' is an invalid argument.".format(command[1])
elif not (command[2] in POSITIONS or command[2] in COLORS): # 2つめの引数は位置を表す番号か色名のどちらか
msg = "'{}' is an invalid argument.".format(command[2])
if msg:
raise Exception(msg)
■ コマンド応じたロボットアームの制御を行う
最後に、recognizeコマンドに対応したロボットアームの制御コードを追加します。
変更・追加【52行目、63行目～71行目】
except Exception as e:
print(e)
continue
else:
if command[0] == "pick":
robotarm.pick()
elif command[0] == "place":
robotarm.place()
elif command[0] == "move":
if command[1] in POSITIONS:
robotarm.move_to(int(command[1]))
else:
robotarm.move_to_color(command[1])
elif command[0] == "transport": # else文からelif文に変更
if command[1] in POSITIONS:
robotarm.move_to(int(command[1]))
else:
robotarm.move_to_color(command[1])
robotarm.pick()
if command[2] in POSITIONS:
robotarm.move_to(int(command[2]))
else:
robotarm.move_to_color(command[2])
robotarm.place()
else: # コマンドが"recognize"のとき
robotarm.pick() # ブロックを持ち上げて色を識別する
color = robotarm.recognize_color()
if color == command[1]: # 1つめの引数に指定した色と同じ場合は、2つめの引数の位置へ移動
if command[2] in POSITIONS:
robotarm.move_to(int(command[2]))
else:
robotarm.move_to_color(command[2])
robotarm.place() # 持ち上げたブロックを置く
■ プログラムの実行
完成したプログラムが以下になります。実行して動作を確認しましょう。
【 サンプルコード 4-1-1 】
from robotarm import RobotArm
COMMANDS = {"pick", "place", "move", "transport", "recognize"} # recognizeを追加
POSITIONS = {"1", "2", "3", "4", "5", "6"}
COLORS = {"grey", "red", "green", "blue", "yellow", "undef"}
robotarm = RobotArm(pin_arm="P13", pin_hand="P14",
pin_body="M1", pin_irp="P0", pin_cs="I2C")
def main():
while True:
try:
data = input("-->")
command = data.split()
msg = None
if not command[0] in COMMANDS:
msg = "'{}' is an invalid command.".format(command[0])
elif command[0] == "move":
if not len(command) == 2:
msg = "'move' only needs 1 argument."
if not (command[1] in POSITIONS or command[1] in COLORS):
msg = "'{}' is an invalid argument.".format(command[1])
elif command[0] == "transport":
if not len(command) == 3:
msg = "'transport' only needs 2 arguments."
if not (command[1] in POSITIONS or command[1] in COLORS):
msg = "'{}' is an invalid argument.".format(command[1])
if not (command[2] in POSITIONS or command[2] in COLORS):
msg = "'{}' is an invalid argument.".format(command[2])
elif command[0] == "recognize": # recognizeコマンドのとき
if not len(command) == 3: # 2つの引数がか必ず必要になるためリストの要素数は3になる
msg = "'recognize' only needs 2 arguments."
elif not command[1] in COLORS: # 1つめの引数は色名のみ
msg = "'{}' is an invalid argument.".format(command[1])
elif not (command[2] in POSITIONS or command[2] in COLORS): # 2つめの引数は位置を表す番号か色名のどちらか
msg = "'{}' is an invalid argument.".format(command[2])
if msg:
raise Exception(msg)
except Exception as e:
print(e)
continue
else:
if command[0] == "pick":
robotarm.pick()
elif command[0] == "place":
robotarm.place()
elif command[0] == "move":
if command[1] in POSITIONS:
robotarm.move_to(int(command[1]))
else:
robotarm.move_to_color(command[1])
elif command[0] == "transport": # else文からelif文に変更
if command[1] in POSITIONS:
robotarm.move_to(int(command[1]))
else:
robotarm.move_to_color(command[1])
robotarm.pick()
if command[2] in POSITIONS:
robotarm.move_to(int(command[2]))
else:
robotarm.move_to_color(command[2])
robotarm.place()
else: # コマンドが"recognize"のとき
robotarm.pick() # ブロックを持ち上げて色を識別する
color = robotarm.recognize_color()
if color == command[1]: # 1つめの引数に指定した色と同じ場合は、2つめの引数の位置へ移動
if command[2] in POSITIONS:
robotarm.move_to(int(command[2]))
else:
robotarm.move_to_color(command[2])
robotarm.place() # 持ち上げたブロックを置く
チャプター5
おわりに
5. 1 このレッスンのまとめ
このレッスンでは、簡単なコマンドを入力するだけで、ロボットアームの操作が行える機能を開発しました。作成したのは１文のコマンドのみに対応したものでしたが、複数行に渡ってコマンドが書かれたテキストファイルを読み込んで、ロボットアームを制御する仕組みも、これまでに学習したことの組み合わせで開発することができます。レッスンでは取り挙げませんが、もし余裕があればぜひ自分の力で挑戦してみてください。
5. 2 次のレッスンについて
次回のレッスンからは、複数の処理を同時に平行して行うプログラムの作り方を学びます。これまでのプログラムとはどのように違うのかを、様々な作例を通して確認していきましょう。