実際のパズルを解くためのプログラムの最後のステップはグリッパーで円盤を動かしパズルを解く処理です。 すでに、「EV3でハノイの塔を その４」において、解法のアルゴリズムを下記の疑似的なpythonの関数solve(n, sc, dc)で記述しましたが、その中のmoveTargetDisc(n, sc, dc) という関数がその処理に相当します。
def solve(n, sc, dc): if n > 1: solve(n-1, sc, 3-sc-dc) moveTargetDisc(nt, n, sc, dc) if n > 1: solve(n-1, 3-sc-dc, dc) def moveTargetDisc(nt, n, sc, dc): #対象の円盤を目的柱に移動する処理
この関数の中で、実際にX、Y、Zの3軸のモーターを回転させて、グリッパ―を目標の位置に移動し、円盤を把持・開放してパズルを解くのですが、 以下にその具体的な実装について説明します。
ハノイの塔のパズルを記述するクラスTHSを以下のように定義する。
class THS: def __init__(self, nd): self.action_Number = 0 self.numDiscTotal = nd self.discAtPoint = [[-1]*3]*nd # [何枚][柱番号] for i in range(nd): # 0 から Discの枚数-1 まで self.discAtPoint[i] = [i, -1, -1] # 柱0に円盤0から円盤nd-1が重なっている self.numDiscColumn = [nd, 0, 0] # 柱0にnd枚ある self.x_MotorAngle = [0, 429, 858] self.y_MotorAngle = [120, 100, 80, 60, 30, -10, -50, 0, 0, 450] self.z_MotorAngle = [0, 180, 335, 490, 645, 800, 955, 1110, 1265, 1420]
アトリビュート
action_Number: 手数のカウント
numDiscTotal : 円盤の枚数 （３から７を想定）
discAtPoint: 1次元目の要素数７，2次元目の要素数３の2次元のリスト。1次元目の要素番号は下から数えた円盤の厚さを単位とした高さ方向の位置（０から６）、2次元目の要素番号は初期に円盤がある柱を０として柱間隔を単位とした横方向の位置（０から２）。
この2次元リストを使って、どの柱にどの円盤が重なっているかを管理する。ここで、円盤を一番大きいものを０，その上を１，・・・と順に番号を付けて区別する。円盤がない空の状態を-1で表すことにすると、初期状態では0番の柱に７枚の円盤が重なっているので、
となる。
numDiscColumn: 3本の柱にそれぞれ何枚の円盤が重なっているかというデータのリスト。初期状態は[7, 0, 0]
x_MotorAngle: インデックスの柱がグリッパーの位置に移動させるためのモーター軸の角度。[0, 429, 858]
y_MotorAngle: インデックスの円盤に合わせてグリッパーを開くためのモーター軸の角度。円盤５，６に対する値がマイナスになっているのは、小さな円盤を滑らないように掴むためには、グリッパー先端が接する状態[ 0 ]より、強く締める必要があるからである。[120, 100, 80, 60, 30, -10, -50, 0, 0, 400]
z_MotorAngle: インデックスの高さの円盤に合わせてグリッパーをZ軸方向で位置決めするためのモーター軸の角度。[0, 180, 335, 490, 645, 800, 955, 1110, 1265, 1420]
このクラスTHSに対してroboというインスタンスを生成し、前述した関数solve(n, sc, dc)を拡張したsolve(robo: THS, np: int, sc: int, dc: int)を定義することにより以下に示すプログラムで実際のパズルを解くことができる。
def solve(robo: THS, np: int, sc: int, dc: int): if np > 1: solve(robo, np - 1, sc, 3-sc-dc) moveTargetDisc(robo, sc, dc) if np > 1: solve(robo, np - 1, 3-sc-dc, dc) def moveTargetDisc(robo, sc, dc): sz = robo.numDiscColumn[sc] - 1 #source columnに重なっている個数 move_horizontally(robo, sc) # 現在の位置からSCの位置に動かす pickup_disc(robo, sc) move_horizontally(robo, dc) # SCの位置からDCの位置に動かす pushdown_disc(robo) sz = robo.numDiscColumn[sc] # 移動元に何枚重なっているか？ dz = robo.numDiscColumn[dc] # 移動先に何枚重なっているか？ robo.discAtPoint[dz][dc] = robo.discAtPoint[sz-1][sc] robo.discAtPoint[sz-1][sc] = -1 # 移動元を空にする robo.numDiscColumn[sc] = robo.numDiscColumn[sc] - 1 # 移動元は1枚減らす robo.numDiscColumn[dc] = robo.numDiscColumn[dc] + 1 # 移動先は1枚増やす robo.action_Number = robo.action_Number +1 def move_horizontally(robo, xd): x_angle = robo.x_MotorAngle[xd] - motor_x.angle() # 柱間をいくつ動かすか？ motor_x.run_angle(360, x_angle, then=Stop.HOLD, wait=True) def pickup_disc(robo, sc): gz_temp = robo.numDiscColumn[sc] - 1 # どの高さのDiscを掴むか z_angle = robo.z_MotorAngle[gz_temp] - motor_z.angle() # どれだけ下げるか？ -は下向き motor_z.run_angle(420, z_angle, then=Stop.HOLD, wait=True) disc_no = robo.discAtPoint[robo.numDiscColumn[sc]-1][sc] # Discの番号 y_angle = robo.y_MotorAngle[disc_no] - motor_y.angle() # Grip用のY軸Motorを何度回すか。 motor_y.run_angle(360, y_angle, then=Stop.HOLD, wait=True) # power×angle<0 で閉じる。 z_angle = robo.z_MotorAngle[9] - motor_z.angle() motor_z.run_angle(420, z_angle, then=Stop.HOLD, wait=True) # どれだけ上げるか？ ＋は上向き def pushdown_disc(robo): z_angle = 1100 - motor_z.angle() # 1100の位置まで下げる motor_z.run_angle(420, z_angle, then=Stop.HOLD, wait=True) y_angle = robo.y_MotorAngle[9] - motor_y.angle() # 9の位置まで開く motor_y.run_angle(360, y_angle, then=Stop.HOLD, wait=True) #power×angle>0 で開く。 z_angle = robo.z_MotorAngle[9] - motor_z.angle() # 上限まで上げる ＋は上向き motor_z.run_angle(420, z_angle, then=Stop.HOLD, wait=True)
下は、33分くらいかかった一連の処理を3分半ほどにまとめたビデオです。
