toio core cube用pythonライブラリtomotoio(改)の解説


# そもそもtomotoioとは
　[tomoto](https://github.com/tomoto)さん作のpython3用toio core cube制御ライブラリです。
Gitリポジトリ https://github.com/tomoto/tomotoio

　SIEからtoio本体セットとtoio Collectionが発売された2019年3月から4ヶ月後の7月にリリースされました。

　toioは技術仕様が最初から開示されていたため、すぐにpythonのライブラリ実装がいくつか行われましたが、個人的に一番使い勝手がよいなと思ったのが[tomotoio](https://github.com/tomoto/tomotoio)でした。

TomotoさんのQiita記事

https://qiita.com/tomoto335/items/2672cbba8f13a9dcfb43

https://qiita.com/tomoto335/items/eb6b814b5ebc08b0e82a


　オリジナルの[tomotoio](https://github.com/tomoto/tomotoio)は、[toio core cube BLE通信仕様](https://toio.github.io/toio-spec/docs/ble_communication_overview) v2.0.0に対応しており、のちにv2.1.0が出た際に対応のための微修正が加わっています。
　ただ、v2.1.0で追加された目的地指定つきモーター制御コマンドへの対応はされていなかったため、個人的に目的地指定つきモーター制御コマンドを使えるように改造して使っていました。
　また、[toio core cube BLE通信仕様](https://toio.github.io/toio-spec/docs/ble_communication_overview) がv2.2.0、v2.3.0とバージョンアップするごとに、モーター速度、磁気センサー、姿勢などの情報もとれるようになり、こちらも対応を入れて使っていました。

　今回、toio Advent Calendar 2021のネタとして、使いやすく機能追加も簡単だった[オリジナルtomotoio](https://github.com/tomoto/tomotoio)への感謝の気持ちを込めまして、長らく手元で使ってきた[私家版tomotoio](https://github.com/kenichi884/tomotoio)を整理して公開し、[オリジナルtomotoio](https://github.com/tomoto/tomotoio)からの追加部分について解説します。

Gitリポジトリ
　https://github.com/kenichi884/tomotoio

# tomotoioを使ううえでの留意点

## 実行環境
　tomotoioライブラリの先は[bluepy](https://github.com/IanHarvey/bluepy)を使っているので、bluepyが使えるUni*環境であればどの環境でもだいたい動きます
　ただし、navigatorクラスについては制御タイミングの都合上、RaspberryPi4B、400、3Bでの動作しか確認できていません。手元のRyzen5-2600のPCで動かしたUbuntu18ではtoio core cubeをうまく動かすことができませんでした。
　cubeクラスについては、問題なくbluepyが使えるUni*環境で動かせています。

### 手元での動作実績 

- Raspberry Pi 3B オンボードBluetooth  Raspbian 2020年初頭ごろのバージョン(たぶんraspbian_lite-2020-02-07)
- Raspberry Pi 4B 4GB オンボードBluetooth Ubuntu20.04 64bit版
- Raspberry Pi 400 オンボードBluetooth Raspberry Pi OS 64bit版(raspios_arm64-2021-11-08)
- Jetson Xavier Xavier NX　開発者キット オンボードBluetooth JetPack4.6(Ubuntu18)
- Jetson nano 開発者キット(A01)  外付けUSB-Bluetoothドングル(CSR8510 A10) 
    - 注:USB3.0ポートに直に挿すとノイズのせいかまともに動きません。USB延長ケーブルなどを使ってUSB-BluetoothドングルをJetson nanoから離す必要があります。
- PC Ryzen5-2600 Ubuntu18.04　　外付けUSB-Bluetoothドングル(CSR8510 A10)

　toio core cubeの同時接続、同時動作は環境によりますが2～4個程度が上限となります。5個つなげなくもないですが安定しません。多数つなぎたい場合はBluetoothインタフェースのほうを増やすなどの工夫が必要です。

## tomotoioのクラス

　以下に説明するのはtomotoioを使ううえでこれだけ見とけばとりあえずOKなクラスです。具体的な使い方はexamples/のサンプルコードを読めばわかります。

### [Cube](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/cube.py)クラス
　toio core cubeの制御を行うクラスです。[toio core cube BLE通信仕様](https://toio.github.io/toio-spec/docs/ble_communication_overview)に沿った形でBLEキャラクタリスティックが提供している機能に対し、write、read、およびnotifyの登録と受信が可能です。
　キャラクタリスティックへのwriteはメソッド名 setXXX()でwriteします。[examples/simpleconfig.py](https://github.com/kenichi884/tomotoio/blob/master/examples/simpleconfig.py)などが参考になります。
  キャラクタリスティックへのreadは、Cube.toioID.get()、Cube.motion.get()、Cube.button.get()などで行います。[examples/simplequery.py](https://github.com/kenichi884/tomotoio/blob/master/examples/simplequery.py)が参考になります。
  notifyはaddListener()でnotifyを受けるハンドラを登録することでnotifyを受信します。[examples/notifications.py](https://github.com/kenichi884/tomotoio/blob/master/examples/notifications.py)が参考になります。
　メソッドは基本的にnon blockingですぐ返ります。

### [Navigator](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/navigator.py)クラス
　マットの座標指定つきモーター制御が使えなかった(左右のモーターの速度のみ)ころのtoio core cubeで、座標指定の移動、超信地旋回、円軌道の移動を行えるようにしたものです。
　今はtoio core cube自身で座標指定の移動ができるようになったので[Navigator](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/navigator.py)クラスの出番も減りましたが、軌道生成コマンドをNavigationCommandBaseクラスから派生して追加できるようになっているので、数学的にモーター速度を算出しておもしろい軌道を作るような用途には向いています。また、マットの境界線からのはみ出しチェックの機能もあります。
　ただし、モーター速度と時間のタイミングの都合上、RaspberryPi4Bに合わせて調整されているため、他の環境ではうまく動作させることができません。

### [Mat](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/navigator.py)クラス
　マット(toio Collection付属)の絶対座標を扱うクラスです。
　topleftで左上座標(x,y)を、bottomrightで右下座標を、centerで中心点座標を知ることができます。
　また、margin()で座標を内側に狭く設定することもできます。
　ただし、この座標およびmargin()で設定した座標については[Navigator](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/navigator.py)クラスを使う場合は境界線を超えないように制御されますが、[Cube](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/cube.py)クラスを使う場合は制限されません。

### [Vector](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/geo.py)クラス
　(x,y)の2次元ベクトルを扱うクラスです。
　ベクトルの足し算や正規化他のベクトル演算を行えます。

## データ型、定数定義
　[data.py](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/data.py)に定義があります。移動タイプ、速度変化タイプ、notifyで得られる情報の値などの定義が書いてあります。

## 引数の単位
　[Cube](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/cube.py)クラスのメソッドでは、時間は秒、角度は度(degree)です。また、座標はintです。(Navigatorクラスでは座標はfloat、角度もfloat)

## notifyを受信する方法
　[Cube](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/cube.py)クラスのaddNotify()で受けたいnotifyのハンドラを追加し、enableNotification()で有効化します。
　[examples/notifications.py](https://github.com/kenichi884/tomotoio/blob/master/examples/notifications.py)が参考になります。
　複数のtoio core cubeを扱っている場合、ハンドラの中でどのtoio core cubeからnotifyかを判別する必要があります。
　また、notifyは兼用(Union)になっているものがあるので判別する必要があります。(motion notifyは、磁気、姿勢と兼用、motor notifyは位置に到達およびモーター速度が兼用)

# pythonライブラリtomotoio(改)の解説 
 ここではオリジナルの[tomoto/tomotoio](https://github.com/tomoto/tomotoio)と、改造版[kenichi884/tomotoio](https://github.com/kenichi884/tomotoio)との違いについて説明します。

## [Cube](https://github.com/kenichi884/tomotoio/blob/master/tomotoio/cube.py)クラス追加メソッド
　以下のメソッドを追加しています。

### ターゲット指定移動
　toio core cube BLE通信仕様の[目標指定付きモーター制御](https://toio.github.io/toio-spec/docs/ble_motor#%E7%9B%AE%E6%A8%99%E6%8C%87%E5%AE%9A%E4%BB%98%E3%81%8D%E3%83%A2%E3%83%BC%E3%82%BF%E3%83%BC%E5%88%B6%E5%BE%A1)に対応します。
　引数はtoio core cube BLE通信仕様の単位そのままです。タイムアウト値の単位は秒、角度の単位は度になります。
　toio core cube BLE通信仕様では目標地点でのキューブの角度と角度変化タイプをUint16一つで与えますが、このメソッドでは角度変化タイプと角度値の２つにわけています。

```python
setMotorWithTarget(制御識別値 int,
                   目標地点のX座標値 int, 目標地点のY座標値 int,
                   モーターの最大速度指示値 int = 10,
                   モーターの速度変化タイプ int = SpeedChangeType.CONSTANT,
                   移動タイプ int = MovementType.ROTATING,
                   目標地点でのキューブの角度変化タイプ int = TargetPointAngleType.NO_ROTATION,
                   目標地点でのキューブの角度 int = 0, 
                   タイムアウト int = 0):
```

### 複数ターゲット指定移動
　toio core cube BLE通信仕様の[複数目標指定付きモーター制御](https://toio.github.io/toio-spec/docs/ble_motor#%E8%A4%87%E6%95%B0%E7%9B%AE%E6%A8%99%E6%8C%87%E5%AE%9A%E4%BB%98%E3%81%8D%E3%83%A2%E3%83%BC%E3%82%BF%E3%83%BC%E5%88%B6%E5%BE%A1)に対応します。
　引数はtoio core cube BLE通信仕様の単位そのままです。タイムアウト値の単位は秒、角度の単位は度になります。

```python
setMotorWithMultipleTargets(目標地点のX座標値 int,
                            目標地点のリスト,
                            モーターの最大速度指示値 int = 10,
                            モーターの速度変化タイプ int = SpeedChangeType.CONSTANT, 
                            移動タイプ  int = MovementType.ROTATING,
                            目標地点追記モード int = AdditionalWriteSettingType.ADD,
                            タイムアウト  int = 0):
```
目標地点のリストはTargetクラス＝4つ組タプル(全部int)
　(x座標, y座標, 目標地点でのキューブの角度変化タイプ, 目標地点でのキューブの角度)　
のリストになります。

```python
例
# x, y, AngleType, deg  
goals = [(250, 250, TargetPointAngleType.NO_ROTATION, 0),
         (200, 250, TargetPointAngleType.ABSOLUTE_FORWARD, 90), 
         (200, 200, TargetPointAngleType.NO_ROTATION, 180)]
```
　[複数目標指定付きモーター制御](https://toio.github.io/toio-spec/docs/ble_motor#%E8%A4%87%E6%95%B0%E7%9B%AE%E6%A8%99%E6%8C%87%E5%AE%9A%E4%BB%98%E3%81%8D%E3%83%A2%E3%83%BC%E3%82%BF%E3%83%BC%E5%88%B6%E5%BE%A1)の仕様では、29個まで目標地点を指定できる、とあるためライブラリでも29個までは指定可能にしていますが、環境によってはBLEのデフォルトのパケットサイズを超えて書き込むことができないため、せいぜい2個までしか有効になりません。手元の環境(RaspberryPi、PC Ubuntuなど)では目標地点3つ以上の指定は無効でした。(setMTU()で拡張できるかと思ったんですが、書いても効いていないみたいです。目標地点3つ以上の指定で動かせた方はぜひ情報をお寄せください。)

### 加速度変化指定移動
　[加速度指定付きモーター制御](https://toio.github.io/toio-spec/docs/ble_motor#%E5%8A%A0%E9%80%9F%E5%BA%A6%E6%8C%87%E5%AE%9A%E4%BB%98%E3%81%8D%E3%83%A2%E3%83%BC%E3%82%BF%E3%83%BC%E5%88%B6%E5%BE%A1)に対応します。
　引数は制御時間は秒ですが、それ以外はtoio core cube BLE通信仕様の単位そのままです。

```python
setMotorWithAcceleration(キューブの並進速度 int,
                         キューブの加速度 int,
                         キューブの進行方向 DirectionType.FORWARD,
                         キューブの向きの回転速度 int = 0,
                         キューブの向きの回転方向 int = DirectionType.FORWARD,
                         優先指定 int = SpeedPriorityType.TRANSITION,
                         制御時間 float = 0):
```

### 読み取りセンサーの ID 通知設定
　[読み取りセンサーの ID 通知設定](https://toio.github.io/toio-spec/docs/ble_configuration#%E8%AA%AD%E3%81%BF%E5%8F%96%E3%82%8A%E3%82%BB%E3%83%B3%E3%82%B5%E3%83%BC%E3%81%AE-id-%E9%80%9A%E7%9F%A5%E8%A8%AD%E5%AE%9A) に対応します。
　引数は最小通知間隔は秒ですが、それ以外はtoio core cube BLE通信仕様の単位そのままです。

```python
setConfigToioIDNotify(最小通知間隔, 通知条件)
```

### 読み取りセンサーの ID missed 通知設定
　[読み取りセンサーの ID missed 通知設定](https://toio.github.io/toio-spec/docs/ble_configuration#%E8%AA%AD%E3%81%BF%E5%8F%96%E3%82%8A%E3%82%BB%E3%83%B3%E3%82%B5%E3%83%BC%E3%81%AE-id-missed-%E9%80%9A%E7%9F%A5%E8%A8%AD%E5%AE%9A) に対応します。
　引数は通知感度設定値の単位は秒です。

```python
setConfigToioIDMissedNotify(通知感度):
```

### 磁気センサーの有効化
　[磁気センサーの設定](https://toio.github.io/toio-spec/docs/ble_configuration#%E7%A3%81%E6%B0%97%E3%82%BB%E3%83%B3%E3%82%B5%E3%83%BC%E3%81%AE%E8%A8%AD%E5%AE%9A-)に対応します。
　磁気センサーはデフォルトでオフなので、磁気センサーを使いたい場合は先にこれを呼んでおく必要があります。
　引数は通知間隔は秒ですが、それ以外はtoio core cube BLE通信仕様の単位そのままです。

```python
setConfigMagneticSensor(機能設定, 通知間隔, 通知条件)
```

### モーター速度検出の有効化
　[モーターの速度情報の取得の設定](https://toio.github.io/toio-spec/docs/ble_configuration#%E3%83%A2%E3%83%BC%E3%82%BF%E3%83%BC%E3%81%AE%E9%80%9F%E5%BA%A6%E6%83%85%E5%A0%B1%E3%81%AE%E5%8F%96%E5%BE%97%E3%81%AE%E8%A8%AD%E5%AE%9A)に対応します。
　モーター速度の検出はデフォルトでオフなので、モーター速度の検出を使いたい場合は先にこれを呼んでおく必要があります。

```python
setConfigMotorSpeedNotify(enable = 1):
```

### 姿勢角検出の有効化
　[姿勢角検出の設定](https://toio.github.io/toio-spec/docs/ble_configuration#%E5%A7%BF%E5%8B%A2%E8%A7%92%E6%A4%9C%E5%87%BA%E3%81%AE%E8%A8%AD%E5%AE%9A-) に対応します。
　姿勢角の検出はデフォルトでオフなので、姿勢角の検出を使いたい場合は先にこれを呼んでおく必要があります。
　引数は通知間隔は秒ですが、それ以外はtoio core cube BLE通信仕様の単位そのままです。

```python
setConfigHighPrecisionTiltSensor(オイラー角かクォータニオンか, 通知間隔, 通知条件):
```

### 目標指定移動の完了通知(notify)
　Cube.motor.addListener()でハンドラを設定します。後述のモーター速度検知と兼用なのでハンドラ内でtype()などで型をみて移動完了通知の情報(MotorResult)を取り出してください。

### 磁気の通知(notify)
　Cube.motion.addListener()でハンドラを設定します。motionと兼用なのでハンドラ内でtype()で型をみて磁気の情報を取り出してください。

### 姿勢の通知(notify)
　Cube.motion.addListener()でハンドラを設定します。motionと兼用なのでハンドラ内でtype()などで型をみて姿勢の情報を取り出してください。
　[examples/tilt.py](https://github.com/kenichi884/tomotoio/blob/master/examples/tilt.py) が参考になります。

### モーター速度の通知(notify)
　Cube.motor.addListener()でハンドラを設定します。motorと兼用なのでハンドラ内でtype()などで型をみてモーター速度の情報を取り出してください。
　[examples/motor_with_acceleation.py](https://github.com/kenichi884/tomotoio/blob/master/examples/motor_with_acceleation.py) が参考になります。

## 新たに増えた型、定数定義

　新たに増えた型、定数定義をdata.pyに追加しています。合わせてexample/utils.pyも変更しています。
追加した型(クラス)

    MotorSpeed　TiltEuler　TiltQuaternion　MagneticForce
    PostureType　MagneticSenseType　NotifyType
    MovementType　SpeedChangeType　TargetPointAngleType
    DirectionType　SpeedPriorityType　AdditionalWriteSettingType　MotorResult

　Matクラスにゲズンロイドのマット、ピコトンズのマット、toio core cube単体販売版付属マット、開発者マットの(左上および右下)座標を追加しています。
　また、MatクラスにSetRect()を追加しています。これは開発者マットをつなぎ合わせた場合など、任意の左上右下座標をセットしたいこともあるので追加しました。

## examples追加

　[examples/](https://github.com/kenichi884/tomotoio/tree/master/examples)に追加した機能のサンプルコードを足しています。

- [motor_with_target.py](https://github.com/kenichi884/tomotoio/blob/master/examples/motor_with_target.py): １番目のキューブを目標地点付き移動で動かす例です。目標座標を２つしか指定していませんが複数目標の例も入っています。
- [motor_with_multiple_targets.py](https://github.com/kenichi884/tomotoio/blob/master/examples/motor_with_multiple_targets.py): １番めのキューブを３個以上の目標地点に沿って動かす例です。setMotorWithMultipleTargets()の追加モードをつかって、最初に2つの目標座標をキューブに積み、最初に積んだ目標座標に到達したところで新たに次の目標座標を積むという方法をとっています。
- [motor_with_acceleation.py](https://github.com/kenichi884/tomotoio/blob/master/examples/motor_with_acceleation.py): １番目のキューブを加速度指定つき移動で動かしています。モーター速度の検出の例でもあります。
- [notifications_mag_posture.py](https://github.com/kenichi884/tomotoio/blob/master/examples/notifications_mag_posture.py): １番目のキューブの磁気センサーの値の検出、姿勢の検出の例です。
- [tilt.py](https://github.com/kenichi884/tomotoio/blob/master/examples/tilt.py): 姿勢検出の例で、１番目のキューブを傾けることで２番目のキューブの前進、後退、右旋回、左旋回を行います。

# さいごに
　文字の説明ばっかりですみません。
　[toio™で作ってみた！友の会（非公式）](https://www.facebook.com/groups/toiotomo/)の活動でもこのpythonライブラリtomotoioを使っていろいろな作品作らせていただいています。たまたまなのですが、「toio友」と「tomotoio」ととても名前が似ているので大変親近感を感じております。[tomoto](https://github.com/tomoto)さんに感謝。