「toio SDK for Unity」でCubeを車輪の角速度[rad/sec]で制御する方法

「ロボットやろうぜ！- toio & Unity 作品動画コンテスト -」というのにアプリを出したのですが，それで「toio SDK for Unity」を触ったのでTipsとか書くシリーズ第二弾です．

コンテストに出したアプリについてはこちら： mu-777.hatenablog.com

コードのレポジトリはこちら： github.com

タイトルの，「Cubeの車輪の角速度[rad/sec]での制御」にちょっと手こずったというか，「toio SDK for Unity」にそのものズバリな関数は用意されていなかったので自作した件について書きます．

ちなみに実機は持っていないので，純粋にUnity Editor上でシミュレータとして使う用途に関する話だけになります．

背景

Cubeを移動させるのは以下のドキュメントのようにCube.Move()関数でできるのですが，引数で指定する左右のモータ速度が0~100の値，となっています

なので，[rad/sec]などの単位で車輪の回転角速度を指定できません．

github.com

正直今回のアプリではそこまで厳密に制御する必要はないのですが，ちゃんとロボットを制御しようとするとすぐに必要になるので[rad/sec]で制御できるようにしておきました．

Cubeを車輪の角速度[rad/sec]で制御する方法

「toio SDK for Unity ドキュメント」の方には書いてなかったと思うのですが（ちゃんと読めてないだけかもですが），「toio コアキューブ技術仕様」の方に，速度指示の値が[rpm]にどう対応しているのか書かれています・

これが「toio SDK for Unity」におけるCube.Move()関数の値と同じであることを信じて参考にします．ここが実は嘘だった，とかならごめんなさい．

f:id:mu-777:20210117191727p:plain — モーター · toio™コアキューブ技術仕様

まず[rad/sec]を[rpm]に変換し，さらに速度指示値に変換してからCube.Move()関数を呼ぶような処理を，Cubeクラスの拡張メソッドとして定義します．

public static class ToioUtils
{
   public static void MoveWithRadPerSec(this Cube self, float left_radPerSec, float right_radPerSec,
                                         int durationMs, Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Weak)
    {
        Func<float, float> radPerSec2rpm = (float radPerSec) => {
            return Mathf.Sign(radPerSec) * Mathf.Clamp(Mathf.Abs(radPerSec * 30.0f / Mathf.PI), 34.0f, 494.0f);
        };
        Func<float, int> rpm2inputval = (float rpm) => {
            return (int)(rpm * (107.0f / 460.0f) + (42.0f / 460.0f));
        };

        self.Move(rpm2inputval(radPerSec2rpm(left_radPerSec)),
                  rpm2inputval(radPerSec2rpm(right_radPerSec)), 
                  durationMs, order);
    }
}