今までの記事
(※2020年1月追加) CoreCubeのfirmwareがアップデートされた！
toio CoreCube のfirmware がアップデートされ、機能追加と安定性向上が図られました。（2020年1月前後）
https://toio.io/update/
この新しいアップデートでは、GATT Handle が変更されています。利用するBLEのライブラリによっては気にする必要はないのですが、bluepy はもろに影響を受けました。具体的には、以下のような変更です。
# BLE Version = ['2.0.0', '2.1.0']
HANDLE_TOIO_ID = [0x0d, 0x0d]
HANDLE_TOIO_MTR = [0x11, 0x11]
HANDLE_TOIO_LED = [0x14, 0x15]
HANDLE_TOIO_SND = [0x17, 0x18]
HANDLE_TOIO_SEN = [0x1a, 0x1b]
HANDLE_TOIO_BTN = [0x1e, 0x1f]
HANDLE_TOIO_BAT = [0x22, 0x23]
HANDLE_TOIO_CFG = [0x26, 0x27]
ということで、BLE Version を取り出して、Handle をダイナミックに変更するように CoreCubeクラス（自作）を修正しました。
また、モーターコントロールにいろいろな機能が追加されています。いろいろありすぎて、どのように対応するべきか、ちょっと考えてから、coreCubeクラスを修正しようかなぁと思っていますので、少々お待ちください。
今回やったこと
今回は、toio ビジュアルプログラミングで提供されている SCRATCH のコマンドの「指定した(X座標, Y座標)に動かす」とか、「xx度に向く」というようなコマンドを実現した。
もともと、技術仕様で公開されているコマンドは、単に車輪の速さを指定するだけで、指示された位置に移動することや、指定角度に回転するコマンドがない。
toio には、技術仕様だけではなく、ビジュアルプログラミングのソースコードも公開されている。こちらを見て、どうやっているのかを確認したのだが、正直、よくわからなかった。（やっぱり、JavaScriptはよくわからない・・・） わかったことと言えば、目的の位置や角度になるまで、両輪の値を変えるようなループにしているということ。
ということで、自分で考えることにした。
- turnTo メソッド（指定された角度を向く）
- moveTo メソッド（指定された座標に移動する）
前準備
必要な環境は、１回目のまま。
この他、トイオコレクションのマットが必要になる。
turnTo メソッド
コアキューブとのもろもろの通信部分を、CoreCubeクラスにまとめている。今回は、turnTo メソッドとして、指定角度に向くメソッドを実現。
サンプルコード
まずは、サンプルコードから。以下のような動作をするソースコード
- コアキューブを270度に向けて、sound id = 4 を再生する。
- これを２秒ごとに、１０回繰り返す。
from coreCube import CoreCube
import time
import sys
toio_addr = CoreCube.cubeSearch()
if len(toio_addr) != 1:
print("1台のコアキューブの電源を入れてください")
sys.exit()
toio = CoreCube()
toio.connect(toio_addr[0])
time.sleep(1)
for k in range(10):
toio.turnTo(270) # 角度を270度にする
toio.id() # XY座標、角度を読み込む
print("dir = %d" % toio.dir)
toio.soundId(4)
time.sleep(2)
toio.disconnect()
CoreCubeクラスに cubeSearch() という、近くのコアキューブを見つけるメソッドも追加した。使い方は、github の方に記載していく予定なので、そちらを参照していただくとして、基本的な使い方は、上記の例の通り。（ちなみに、root で実行する必要がある）
turnTo()メソッドの中身は、こんな感じ。
# 指定角度を向く
def turnTo(self, tdir):
for i in range(20): # --- ２０回繰り返す
self.id()
diff_dir = tdir - self.dir
if (abs(diff_dir) <= 15 ): # --- 目的の角度に近くなったら停止
self.motor( (0, 0), 0)
break
else:
if diff_dir > 180: diff_dir -= 360
if diff_dir < -180: diff_dir += 360
sp = max(int(diff_dir/4), 10) if diff_dir > 0 else min(int(diff_dir/4), -10)
self.motor( (sp, -sp), 10)
time.sleep(0.02)
考え方としては、現在の角度と目的の角度の差を計算し、回転方向と回転速度を決めている。差が大きければ、回転速度を早くしている。また、モーターの最低速度は 10なので、それより小さくならないようにしている。
正直、きれいなコードではない。２行目のrange(20)は繰り返し回数だし、最下行のtime.sleep(0.02)はループ間隔を固定で書いているし、５行目の 15 は、回転を停止する角度が固定になっているし、下から３行目の /4 も適当な数字だし・・・。
実は、これらの値は、何度も試してみて、一番良さげな値にしたつもり。こういうRealなものの動きを制御するには、理論通りにはなかなかいかないところが、難しいということか・・・。（ただ、我が家にある２つのコアキューブで確認しただけなので、すべてのコアキューブでうまくいくとは限らないので、注意。）
サンプルの実行
$ sudo python3 sample_turnto.py # 一番近くのコアキューブに自動接続
dir = 272
dir = 267
dir = 266
dir = 270
dir = 267
dir = 267
dir = 266
dir = 272
dir = 267
dir = 268
実際に実行してみると、意外と、近いところで止まっている。誤差は+/-5ぐらいになった。
turnToメソッド内で、停止条件を +/-15以下にしたのに、より小さく収まっているのは、そんなすぐには止まらないということかな。
moveTo メソッド
moveTo メソッドは、指定したXY座標にコアキューブを移動させるメソッド
サンプルコード
以下のような動作をするソースコード
- (100, 100) に速度60 で移動し、sound id = 3 を再生する。
- これをXY座標を変えて、数回繰り返す。
from coreCube import CoreCube
import time
import bluepy
import sys
toio_addr = CoreCube.cubeSearch()
if len(toio_addr) == 0:
print("コアキューブの電源を入れてください")
sys.exit()
toio = CoreCube()
toio.connect(toio_addr[0])
time.sleep(1)
toio.moveTo(100, 100, 60)
toio.id()
print("(x, y) = (%d, %d)" % (toio.x, toio.y) )
toio.soundId(3)
time.sleep(1)
toio.moveTo(400, 400, 60)
toio.id()
print("(x, y) = (%d, %d)" % (toio.x, toio.y) )
toio.soundId(3)
time.sleep(1)
toio.moveTo(100, 300, 60)
toio.id()
print("(x, y) = (%d, %d)" % (toio.x, toio.y) )
toio.soundId(3)
time.sleep(1)
toio.moveTo(300, 100, 60)
toio.id()
print("(x, y) = (%d, %d)" % (toio.x, toio.y) )
toio.soundId(3)
time.sleep(1)
toio.disconnect()
moveTo()メソッドの中身は、こんな感じ。
# 指定位置に向かう
def moveTo(self, tx, ty, speed):
STOP = 10
SLOW = speed
while True:
if self.id() != 1:
break
# --- 指定位置に近づいた？
dist = math.sqrt((tx - self.x)**2 + (ty - self.y)**2)
ds = 1.0 # --- 減速係数
if dist < STOP: # --- 指定位置からSTOP範囲に入ったら、終了
break
if dist < SLOW: # --- 指定位置からSLOW範囲に入ったら、減速
ds = max(dist / SLOW, 0.3)
# --- 現在位置から、指定位置までの角度を計算
tdir = int(math.acos( (tx - self.x) / dist ) * 180.0 / math.pi)
if ty - self.y < 0: tdir *= -1
# --- 角度補正値計算
diff_dir = tdir - self.dir
if diff_dir > 180: diff_dir -= 360
if diff_dir < -180: diff_dir += 360
dr = abs(int(diff_dir / 2)) + 1 if diff_dir > 0 else 0
dl = abs(int(diff_dir / 2)) + 1 if diff_dir < 0 else 0
# --- move
sl = max(int((speed - dl) * ds ), 10)
sr = max(int((speed - dr) * ds ), 10)
if sl+sr == 20: # --- 最低速度だと差が出ないので・・・
sl = sl + 1 if dr != 0 else sl
sr = sr + 1 if dl != 0 else sr
self.motor( (sl, sr), 0 )
time.sleep(0.03)
self.motor( (0, 0), 0 )
こちらの考え方としては、
- 現在の位置と目的の位置の距離を計算し、STOP閾値よりも小さければ、目的についたとして停止する
- 距離がSLOW閾値よりも小さくなったら、減速させる（減速係数を計算）
- 今向いている方向と、現在位置と目的の位置の角度の差を計算し、左右の車輪の速度差を決定する
- 左右の速度差と、減速係数をもとに、車輪の速度を決めて、モーターを回す
- この繰り返し
これも、いろいろ試してみた結果となっている。
特に、減速については、移動速度を速くすると、うまく止まらず、目的値を通り過ぎてしまったりするため、考慮が必要になった。また、減速区間についても、４行目の SLOW = speed が表すように、固定値ではなく、速度によって変えた。これで、かなり近いところで止まるようになった。
サンプルの実行
$ sudo python3 sample_moveto.py
(x, y) = (104, 100)
(x, y) = (396, 396)
(x, y) = (104, 302)
(x, y) = (296, 103)
こちらも、誤差は+/-5ぐらいになった。
今後
ここまでの３回で、コアキューブの基本的な動作については実現できたと思う。
ただ、実際のゲームを考えると、複数のキューブを座標を読み取りながら、処理を行わせる必要がある。bluepy では、 .waitForNotifications() というメソッドで、Notify 待ちをしなければならない。それを、それぞれのインスタンスで同時に行わなければならず、シングルスレッドでは不可能。なので、今回のサンプルで、何気なく使った id()メソッドでは、Notifyではなく、ReadでID情報を読み込んメソッドを用意した。
しかし、ボタンを押したときや、コアキューブをたたいた時は Notifyでなければ読み取れないので、何らかの手段でこのNotifyイベントを処理しなければならない。Python でどうやって実現するのがいいのか、調べて、試してみたい。