The Fool In The Valleyの雑記帳

-- 好奇心いっぱいのおじいちゃんが綴るよしなし事 --

EV3でハノイの塔を その8

実際のパズルを解くためのプログラムの最後のステップはグリッパーで円盤を動かしパズルを解く処理です。 すでに、「EV3でハノイの塔を その4」において、解法のアルゴリズムを下記の疑似的な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 : 円盤の枚数 (3から7を想定)
discAtPoint:  1次元目の要素数7,2次元目の要素数3の2次元のリスト。1次元目の要素番号は下から数えた円盤の厚さを単位とした高さ方向の位置(0から6)、2次元目の要素番号は初期に円盤がある柱を0として柱間隔を単位とした横方向の位置(0から2)。
f:id:tfitv:20210318110241p:plain この2次元リストを使って、どの柱にどの円盤が重なっているかを管理する。ここで、円盤を一番大きいものを0,その上を1,・・・と順に番号を付けて区別する。円盤がない空の状態を-1で表すことにすると、初期状態では0番の柱に7枚の円盤が重なっているので、

\displaystyle{
\text { discAtPoint }=\left[\begin{array}{ccc}
0 & -1 & -1 \\
1 & -1 & -1 \\
2 & -1 & -1 \\
3 & -1 & -1 \\
4 & -1 & -1 \\
5 & -1 & -1 \\
6 & -1 & -1
\end{array}\right]
}

となる。
numDiscColumn: 3本の柱にそれぞれ何枚の円盤が重なっているかというデータのリスト。初期状態は[7, 0, 0]
x_MotorAngle: インデックスの柱がグリッパーの位置に移動させるためのモーター軸の角度。[0, 429, 858]
y_MotorAngle: インデックスの円盤に合わせてグリッパーを開くためのモーター軸の角度。円盤5,6に対する値がマイナスになっているのは、小さな円盤を滑らないように掴むためには、グリッパー先端が接する状態[ 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分半ほどにまとめたビデオです。

youtu.be