THS1の機構ができ、解法のアルゴリズムが確認できたので、いよいよ実際のパズルを解くためのプログラムの作成です。
パズルを解く処理を、
１）グリッパーとパズルをある範囲内にセットする
２）原点を自動的に設定する
３）グリッパーで円盤を動かしパズルを解く
のステップで考えることにして、今回から3回に分けて説明します。
まず１）ですが、これを実行するためにはグリッパーとパズルの位置関係をラフにマニュアルで調整できると好都合です。その調整はモータ軸を手で回しても可能でしょうが、すこしスマートにやりたいところです。「EV3でハノイの塔を その１~その３」で3軸方向に動かす機構を作った際に、IRリモコンとレシーバを使ってモーターを動かしましたが、その機能を使うと非接触で調整できて楽です。そこで、THS1にもそれを組み込むことにしました。
以下にその概要を記します。
x軸用モーターをMotorPortA、y軸用モーターをMotorPortB、z軸用モーターをMotorPortCに接続する。
IRレシーバーをSensorPort2に接続する。
IRリモコンの1ch左にy軸用モーター、1ch右にz軸用モーター、2ch右にx軸用モーターのコントロールをアサインする。
IRリモコンで3軸のモーターを制御し、位置決めが完了したらブリックのボタンをどれかを押して終了する。
Pythonのプログラムは以下の通り。
#!/usr/bin/env pybricks-micropython from pybricks.hubs import EV3Brick from pybricks.ev3devices import Motor from pybricks.iodevices import I2CDevice from pybricks.parameters import Port, Stop ev3 = EV3Brick() motor_x = Motor(Port.A) motor_y = Motor(Port.B) motor_z = Motor(Port.C) IRR = I2CDevice(Port.S2, 0x01) while True: buff = IRR.read(0x42, 4) #42から4byte読み込みbuff[4]に入れる if buff[0] == 100: #1ch左が前 motor_y.run(300) # medium motorを300deg/sで正回転 if buff[0] == 156: #1ch左が後 motor_y.run(-300) # medium motorを300deg/sで逆回転 if buff[0] == 0: #1ch左が中立 motor_y.hold( ) # medium motorを停止させ固定 if buff[1] == 100: #1ch右が前 motor_z.run(300) # medium motorを300deg/sで正回転 if buff[1] == 156: #1ch右が後 motor_z.run(-300) # medium motorを300deg/sで逆回転 if buff[1] == 0: #1ch右が中立 motor_z.hold( ) # medium motorを停止させ固定 if buff[3] == 100: #2ch右が前 motor_x.run(150) # medium motorを150deg/sで正回転 if buff[3] == 156: #2ch右が後 motor_x.run(-150) # medium motorを150deg/sで逆回転 if buff[3] == 0: #2ch右が中立 motor_x.hold( ) # medium motorを停止させ固定 if any(ev3.buttons.pressed()): # ボタンのどれかが押されたらwhileループをでる break
ビデオは、このプログラムを使ってラフな位置決めをする様子である。 youtu.be
