今までの記事

(※2020年1月追加) CoreCubeのfirmwareがアップデートされた！

toio CoreCube のfirmware がアップデートされ、機能追加と安定性向上が図られました。（2020年1月前後）
　　https://toio.io/update/
この新しいアップデートでは、GATT Handle が変更されています。利用するBLEのライブラリによっては気にする必要はないのですが、bluepy はもろに影響を受けました。具体的には、以下のような変更です。

.py

  # 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 メソッドとして、指定角度に向くメソッドを実現。

CoreCubeクラス

サンプルコード

まずは、サンプルコードから。以下のような動作をするソースコード

コアキューブを270度に向けて、sound id = 4 を再生する。
これを２秒ごとに、１０回繰り返す。

sample_turnto.py

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()メソッドの中身は、こんな感じ。

.py

  # 指定角度を向く
  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()メソッドの中身は、こんな感じ。

.py

  # 指定位置に向かう
  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 でどうやって実現するのがいいのか、調べて、試してみたい。

参考サイト