技術ドキュメント - 使い方 - CubeHandleクラス

目次

1. 概説

Cube クラスには toio™ の仕様通りに基礎機能が実装されています。
それに対し、プログラムからキューブの移動操作をより簡単にできるようにしたのが CubeHandle クラスです。

CubeHandle インスタンスは Cube インスタンスと一対一対応するもので、 Cube を使ってインスタンス化されます。

Cube cube = ...
CubeHandle handle = new CubeHandle(cube);

CubeHandle クラスの内部の構造と Cube クラスとのやり取りは以下の制御ブロック図で示されています。

情報の流れに沿って説明すると

  1. Cube の状態（座標と角度）が Update メソッドによって CubeHandle に取り込まれると、まず予測モジュールに入り、 現在速度やラグ後の状態などを推算します
  2. 予測結果と Cube の状態と合わせて、制御メソッドに入ります
  3. 一つのルートは、ユーザーが指定した目標（座標や角度など）を、 相応の制御メソッド（例えば Move2Target）に渡し、Movement を出力し Move メソッドに渡します
  4. Move メソッドは 3. の Move2Target メソッドの出力した Movement を受けるか、 或いはもう一つのオーバーロードで、 ユーザーの指定した前進指示値と回転指示値と継続時間とを受けて、 デッドゾーン処理とボーダー制限をかけて、Cube.Move メソッドの入力と同じ形式の 左右指示値と継続時間に変換して、MoveRaw メソッドに渡します
  5. MoveRaw メソッドは Cube.Move メソッドと同じ入力形式を持ち、 指示値を弄ったりもせず、ただ単に指令値を予測モジュールに保存させて、 そして Cube.Move メソッドに渡して実行させます


2. CubeHandle クラス API

CubeHandle クラスを使ったサンプルなどはチュートリアルで紹介しています。 そちらも参照してください。

2.1. 変数

定数

public static double TireWidthDot { get; }      // 左右車輪の間隔（マット座標）
public static double VDotOverU { get; }         // 速度と指示値の比例 (dot/sec) / cmd
public static double DotPerM { get; }           // マット単位とメートルの比例 dot / mm
public static readonly float MotorTau = 0.04f;  // モーターの一次遅れ要素 sec
public double deadzone { get; }                 // モーター指示値のデッドゾーン（実例化の際固定される）
public int maxSpd { get; }                      // 最大速度指示値（実例化の際固定される）

パラメーター

public static double dt = 1.0 / 60 * 3;     // 制御の周期 50ms
public static double lag = 0.130;           // キューブから情報取得する際の遅延とモーター指令を送信してからキューブが受信するまでの遅延を合わせた値

public RectInt borderRect   // ボーダーの範囲を表す RectInt

プロパティ

// 現在状態
public Cube cube { get; }   // キューブ
public Vector pos { get; }  // 座標
public double x { get; }    // ｘ座標
public double y { get; }    // ｙ座標
public double rad { get; }  // 弧度
public double deg { get; }  // 角度（度）
public Vector dir { get; }  // 単位方向ベクトル
public int lagMS { get; }   // ラグ（ms）
public int dtMS { get; }    // 制限の周期（ms）
public bool touch { get; }  // タッチ状態（リアルだけ有効）
public bool lost { get; }   // ロスト状態（リアルだけ有効）

// 予測結果
public double spdL, spdR;       // 現在左右タイヤの速度
public double radPred, xPred, yPred, spdPredL, spdPredR;    // ラグ後の弧度、ｘｙ座標、左右速度
public double stopRadPred, stopXPred, stopYPred;    // 停止指示を出す場合、停止した後の弧度、ｘｙ座標
public double spd { get; }      // 現在速度大きさ
public Vector v { get; }        // 現在速度ベクトル
public Vector posPred { get; }  // ラグ後の座標
public double spdPred { get; }  // ラグ後の速度大きさ
public Vector vPred { get; }    // ラグ後の速度ベクトル
public double wPred { get; }    // ラグ後の角速度
public Vector stopPosPred { get; }  // 停止指示を出す場合、停止した後の座標


2.2. Movement 構造体

Movement 構造体は使いやすくするため、制御メソッドの出力を統合した構造体です。

変数

public CubeHandle handle;   // 誰の指示か
public double translate;    // 前進速度指示値
public double rotate;       // 回転速度指示値
public int durationMs;      // 継続時間（ms）
public bool reached;        // 制御メソッド完了したか
public bool idle;           // この Movement が実行されるか

メソッド

Exec

public Movement Exec(bool border=true);

ボーダーの有無を指定し、メンバー変数 handle の Move を呼んで実行します。


2.3. 基本メソッド

SetBorderRect

public void SetBorderRect(RectInt matRect, int margin=20)

マットのサイズを表す RectInt と margin によって、ボーダー borderRect を設定します。

※マットの RectInt は、Mat クラスの GetRectForMatType メソッドに MatType を指定して便利に取得することができます。

Update

public void Update();

状態更新、移動先の予測をします。

このメソッドは MoveRaw 以外のメソッドを実行するフレームで、それらを実行する前に一回実行する必要があります。

MoveRaw

public void MoveRaw(
  double uL, double uR,
  int durationMs = 1000,
  Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Weak
  );

キューブを移動させます。

Move

public Movement Move(
    double translate,           // 前進速度の指示値
    double rotate,              // 回転速度の指示値
    int durationMs = 1000,      // 継続時間（ms）
    bool border = true,         // ボーダー制限のありなし
    Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Weak    // 指示の優先度
    );

キューブを移動させます。

Overloads

public Movement Move(
  Movement mv,
  bool border = true,
  Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Weak
  );

Movement を実行します。

public Movement Move(
  Movement mv,
  int durationMs,
  bool border = true,
  Cube.ORDER_TYPE order = Cube.ORDER_TYPE.Weak
);

Movement の継続時間を書き換えて実行します。

Stop

public void Stop();

キューブを停止させます。

moveRaw(0,0,100,Cube.ORDER_TYPE.Strong) と等価です。


2.4. One-Shot メソッド

CubeHandle クラスの Closed-loop メソッドは目的に到達するために何度も繰り返し実行する想定です。 処理を実行するたびにキューブと Bluetooth 通信をすることなるため、 移動しながら LED を点滅したり、音を鳴らしたりすると通信量が多くなりすぎてしまいます。

One-shot メソッドはこの問題を解決するための機能で、 目標に達するために一回だけ呼び出せば良いので移動をするのに必要な通信量を抑えることが出来ます。 （Open-Loop なので、結果の保証はありません）

TranslateByDist

public Movement TranslateByDist(double dist, double translate);

指定距離を指定速度で前進・後退する Movement を計算します。

RotateByRad

public Movement RotateByRad(double drad, double rotate);

指定角度（弧度）を指定回転速度で回転する Movement を計算します。

RotateByDeg

public Movement RotateByDeg(double ddeg, double rotate)

指定角度（度）を指定回転速度で回転する Movement を計算します。


2.5. Closed-Loop メソッド

繰り返し実行し続けることで、マットの指定した座標、指定した方向に到達するのが Closed-Loop メソッドです。

One-Shot メソッドと異なり 目標を追い続け、結果が保証されますが、高頻度にキューブと通信をするので送信量が多くなります。

Move2Target

public Movement Move2Target(
    double tarX,            // 目標ｘ座標
    double tarY,            // 目標ｙ座標
    double maxSpd = 50,     // 最大速度の指示値
    int rotateTime = 250,   // 希望回転時間（ms）
    double tolerance = 8    // 到達判定の閾値
    );

目標座標に移動する Movement を計算します。

Overloads

public Movement Move2Target(Vector pos, double maxSpd = 50, int rotateTime = 250, double tolerance = 8);
public Movement Move2Target(Vector2 pos, double maxSpd = 50, int rotateTime = 250, double tolerance = 8);
public Movement Move2Target(Vector2Int pos, double maxSpd = 50, int rotateTime = 250, double tolerance = 8)

Rotate2Rad

public Movement Rotate2Rad(double tarRad, int rotateTime = 400, double tolerance = 0.1);

指定角度（弧度）に回転する Movement を計算します。

Rotate2Deg

public Movement Rotate2Deg(double tarDeg, int rotateTime = 400, double tolerance = 5);

指定角度（度）に回転する Movement を計算します。

Rotate2Target

// tarX, tarY 指定座標、tolerance 到達判定の閾値(弧度)、rotateTime 希望回転時間（ms）
public Movement Rotate2Target(double tarX, double tarY, int rotateTime = 400, double tolerance = 0.1);

指定座標の方向に回転する Movement を計算します。