この記事はなに？
私は現在2x2x2ルービックキューブを解くロボットを開発中です。これはそのロボットのプログラムの解説記事集です。
かつてこちらの記事に代表される記事集を書きましたが、この時からソフトウェアが大幅にアップデートされたので新しいプログラムについて紹介しようと思います。
該当するコードはこちらで公開しています。
関連する記事集
「ルービックキューブを解くロボットを作ろう！」
ルービックキューブロボットのソフトウェアをアップデートした
今回は機械操作(Python)編として、controller.pyを紹介します。
初期設定など
最初にシリアル通信やGPIOのセットアップをします。
ser_motor = [None, None]
GPIO.setmode(GPIO.BCM)
GPIO.setup(21,GPIO.IN)
ser_motor[0] = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01, write_timeout=0)
ser_motor[1] = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.01, write_timeout=0)
アクチュエータを動かすコマンドを送る
アクチュエータとはここではつまりモーターです。モーターは自作Arduino互換機に繋がれていて、Arduinoにコマンドを送ることでアクチュエータを動作させます。ここではコマンドを送る関数を紹介します。
''' アクチュエータを動かすコマンドを送る '''
''' Send commands to move actuators '''
def move_actuator(num, arg1, arg2, arg3=None):
if arg3 == None:
com = str(arg1) + ' ' + str(arg2)
else:
com = str(arg1) + ' ' + str(arg2) + ' ' + str(arg3)
ser_motor[num].write((com + '\n').encode())
アクチュエータを動かすコマンドには2種類あって、これによって引数の数が違います。そこでarg3 == Noneを使ってif文を書いています(なら関数を分ければ良いという話ではありますが)。
パズルを掴む/離す
先程の関数を使って、パズルを掴む関数と離す関数を作りました。これは時々行う動作のため関数化してあります。
''' キューブを掴む '''
''' Grab arms '''
def grab_p():
for i in range(2):
for j in range(2):
move_actuator(j, i, 1000)
sleep(3)
''' キューブを離す '''
''' Release arms '''
def release_p():
for i in range(2):
for j in range(2):
move_actuator(i, j, 2000)
2台のArduinoiについて、それぞれに繋がれている2つのモーターjを動かします。
1000や2000というのは、1000を送るとパズルを掴み、2000を送ると離すようにしてあるということです。
アームのキャリブレーション
アームはステッピングモーターで動かしているため、適宜位置を調整してやる必要があります。Arduinoにはホールセンサ(磁気センサ)がついていて、アームには磁石をつけてあります。これを使うことで位置調整を自動で行います。Pythonからはコマンドを送るだけで位置調整ができるようになっています。
''' アームのキャリブレーション '''
''' Calibration arms '''
def calibration():
release_p()
sleep(0.1)
for i in range(2):
for j in range(2):
move_actuator(j, i, 0, 500)
実際にロボットを解法通りに動かす
解法とその他定数を入力するとロボットを動かしてくれる関数です。
ロボットには非常停止ボタンがついていて、まずその処理が行われます。ちなみに非常停止ボタンはプルアップになっているのでコネクタが外れるだけでも動作が止まります。
そして初手以外を回す際にはパズルを持ち替え、順次モーターを回し、最大の回転数に比例した時間だけ休みます。最後に解くのにかかった時間を返します。
''' 実際にロボットを動かす '''
''' Control robot '''
def controller(slp1, slp2, rpm, ratio, solution):
strt_solv = time()
for i, twist in enumerate(solution):
# 非常停止ボタンを押すと止まる
if GPIO.input(21) == GPIO.LOW:
if bluetoothmode:
client_socket.send('emergency\n')
solvingtimevar.set('emergency stop')
print('emergency stop')
return
# パズルの持ち替え
if i != 0:
grab = twist[0][0] % 2
for j in range(2):
move_actuator(j, grab, 1000)
sleep(slp1)
for j in range(2):
move_actuator(j, (grab + 1) % 2, 2000)
sleep(slp2)
max_turn = 0
for each_twist in twist:
move_actuator(each_twist[0] // 2, each_twist[0] % 2, each_twist[1] * 90, rpm)
max_turn = max(max_turn, abs(each_twist[1]))
# パズルが回転するのを待つ
slptim = 2 * 60 / rpm * max_turn * 90 / 360 * ratio
sleep(slptim)
solv_time = str(int((time() - strt_solv) * 1000) / 1000).ljust(5, '0')
return solv_time
まとめ
今回は実際にロボットを動かす関数(と言ってもやっていることはコマンドを送ることですが)を解説しました。次はArduino側でコマンドを受け取ってからどうやってモーターを動かしているのかについて解説します。