Movado は Pico-2 上で動作するモータ制御ソフトウェアです．Infanoid や Keepon に装着された Pico-2 は，この Movado によってモータを制御しています．

Movado は，ホスト PC からシリアル通信線をとおして指令を受けとり，最大 4 つのエンコーダ付き DC モータ（加えて最大 4 つのラジコン用サーボ）を制御します．ひとつのシリアル通信線に最大 31 個の Pico-2（これを「ノード」と呼びます）を接続でき，最大 124 個の DC モータを制御できるので，多自由度のロボットを比較的簡単に構成することができます．PC との接続およびモータとの接続については ClayBot 仕様 のページをご覧ください．

Movado は，Pico-2 の BIOS インタフェース 上に C 言語によってプログラムされています．コンパイルの方法やフラッシュ=メモリへの書き込みについては ソフトウェア開発環境 / フラッシュ=メモリへの書き込みかた のページをご覧ください．

• ダウンロード： 制御ソフト Movado

このページでは，PC からの指令プロトコルを解説します．この指令プロトコルによって，PC からシリアル通信線をとおして，複数のモータを自在に制御することができます．Movado についてのより詳しい情報は，Movado のしくみ のページをご覧ください．

このページの先頭に戻る

Movado への指令は 6 バイトのパケット（packet）の形をとります．下図の左端に縦に並べられたのが 6 バイトのパケットです．パケットの先頭バイト（1st byte）のみ MSB（most signficant bit = bit7）が 1 となり，後続するバイトの MSB は 0 となります．この MSB のパターンによって，シリアル通信におけるパケットのフレーム同期を保証しています．

パケットは，アドレス（addr）・モータ番号（n）・命令（inst），そして 4 バイト引数（quad）から成ります．

アドレス（addr）は 5 ビット（0〜31）で，シリアル通信線に接続されたノード（Pico-2）を識別します．アドレスは Pico-2 基板上のディップスイッチ SW1〜5 で設定します．SW5=on SW4=off SW3=off SW2=on SW1=off ならば，addr=0x12 (18) となります．同じのシリアル通信線上に接続されたノードは，それぞれ異なるアドレスを持たなければなりません．addr=0x1f (31) は「同報アドレス」として予約されているため，Pico-2 のアドレスとして設定することはできません．なお，後述するように，アドレスは３つのクラス（0x00〜0x07, 0x08〜0x0f, 0x10〜0x1e）に分かれ，クラスごとにエンコーダ分解能の自動設定が行なわれます．

モータ番号（n）は 2 ビット（0〜3）で，Pico-2 に接続された最大 4 つのモータ MOT0〜3 を識別します．アドレスとモータ番号をつなげた 7 ビットによって，シリアル通信線上の特定のモータを識別することができます．（ラジコン用サーボに関する副命令 move_rcs・setacc_rcs・setvel_rcs・setpos_rcs に関しては，この n によって 4 つのラジコン用サーボ RCS0〜3 を識別します.）

命令（inst）は 3 ビット（0〜7）です．指令パケットを受け取った Movado は，この命令を実行します．その詳細は後述します．

4 バイト引数（quad）は第 3〜6 バイトから取り出されますが，本来あるべき 4 つの MSB がすべて別の場所に移動されているため，復元作業が必要となります．これらの MSB は，第 2 バイト（2nd byte）の下位 4 ビット（m1〜4）に格納されています．そこで，第 3〜6 バイトの下位 7 ビット（hept1〜4）に，それぞれ m1〜4 を MSB として加え，4 バイト（data1〜4）の列を得ます．これが 4 バイト引数（quad）となります（上図の右側中段）．

この 4 バイト引数（quad）は，命令（inst）に応じて異なる意味づけを受けます．まず inst=0〜3, 5〜7の場合，data1（上位）と data2（下位）を連結した上部 16 ビット（upper）を整数部とし，data3（上位）と data4（下位）を連結した下部 16 ビット（lower）を小数部とする 32 ビット固定小数点データ（fix）となり（上図の右側上段），これを引数として命令（inst）が実行されます．一方，inst=4（sub）の場合は，data1 を副命令（jnst），data2 をインデックス（index），data3（上位）と data4（下位）を連結した 16 ビットをワードデータ（word）からなる副指令（sub-command）が実行されます（上図の右側下段）．副指令の詳細は後述します．

32 ビット固定小数点データ（fix）は，16 ビットの整数部と 16 ビットの小数部によって，およそ -32768〜32768 を範囲とする値をとることができます．表現可能な最小ステップは 2^-16（およそ 0.000015）です．ホスト PC 上の浮動小数点（double など）から 32 ビット固定小数点（fix）への型変換は，以下の C コードを参考にしてください．（ホスト PC での int が 32 ビットでない場合は，若干の変更が必要になります.）

typedef int fix;

fix  double2fix (double d) {
    return (fix) (d * 65536);
}

double  fix2double (fix f) {
    return f / 65536.0;
}

パケットを受け取った Movado は，そのアドレス（addr）が自分のアドレス（または同報アドレス 0x1f）である場合，それを指令（command）として実行します．inst=0〜3 をもつ指令（表中でセルが赤）は，モータを駆動したり，速度・加速度などを設定するためのものです．inst=5〜7 をもつ指令（表中でセルが青）は，Movado のモータの状態（現在の位置・速度・加速度など）を問い合わせるためのもので，Movado から PC に返信パケットが送信されます．inst=4 の場合は，引数に埋め込まれた副指令（sub-command）を実行します．

• move 命令（inst=0）
  n で指定されたモータを，台形速度制御によって位置 pos (fix) [rad] まで回転させます．それ以降，モータはその位置を保持するようになります．このときの加速度（の絶対値）は，後述する setacc 命令によって設定されたものが，また最高速度（の絶対値）は，後述する setvel 命令によって設定されたものが使われます．
• setacc 命令（inst=1 かつ acc ≥ 0）
  n で指定されたモータの move・movel 命令における加速度の絶対値を acc (fix) [rad/s²] に設定します．
• setvel 命令（inst=2 かつ vel ≥ 0）
  n で指定されたモータの move・movac 命令における最高速度の絶対値を vel (fix) [rad/s] に設定します．
• setpos 命令（inst=3）
  n で指定されたモータを，最大限のパワーで位置 pos (fix) [rad] まで回転させます．それ以降，モータはその位置を保持するようになります．
• sub 命令（inst=4）
  quad に記述された副指令（sub-command）を実行します．jnst ≥ 0x80 の場合，Movado は返信パケットを PC に送信します．個々の副指令については後述します．
• getacc 命令（inst=5）
  n で指定されたモータの台形速度制御のために設定された加速度の絶対値 acc [rad/s²] を，元のパケットの fix 領域に格納して PC に返信します．
• getvel 命令（inst=6）
  n で指定されたモータの現在の速度 vel [rad/s] を，元のパケットの fix 領域に格納して PC に返信します．この速度 vel は実測値（アンチエイリアシング後）であり，負の値はマイナス方向（CCW方向）の速さを表わします．
• getpos 命令（inst=7）
  n で指定されたモータの現在の位置 pos を，元のパケットの fix 領域に格納して PC に返信します．

このほかに，move 命令とよく似た movac・movel 命令があります．movac 命令は，加速度（の絶対値）を設定した上で，また movel 命令は，最高速度（の絶対値）を設定した上で，move 命令を実行します．この加速度・最高速度の設定は一時的なもので，後続する move・movac・movel 命令には影響しません．

• movac 命令（inst=1 かつ fix<0）
  n で指定されたモータを，台形速度制御によって位置 pos [rad] まで回転させます．それ以降，モータはその位置を保持するようになります．位置 pos は，整数部を pos_int とし小数部を 0 とする 32 ビット固定小数点データとして与えられます．このときの加速度（の絶対値）acc は，−acc_int を整数部とし 0 を小数部とする 32 ビット固定小数点データ [rad/s²] として与えられます．したがって，movac 命令での upper:16 は常に負の値をとります．また最高速度（の絶対値）は，setvel 命令によって設定されたものが使われます．
• movel 命令（inst=2 かつ fix<0）
  n で指定されたモータを，台形速度制御によって位置 pos まで回転させます．それ以降，モータはその位置を保持するようになります．位置 pos は，整数部を pos_int とし小数部を 0 とする 32 ビット固定小数点データとして与えられます．このときの最高速度（の絶対値）vel は，−vel_int を整数部とし 0 を小数部とする 32 ビット固定小数点データ [rad/s] として与えられます．したがって，movel 命令での upper:16 は常に負の値をとります．また加速度（の絶対値）は，setacc 命令によって設定されたものが使われます．

なお，ここでいう「位置」とは，エンコーダが連結されたモータ単体の出力軸上の回転位置 θ [rad] となります．モータ単体を出力軸側から見たとき時計回り（CW）となるのがプラスの方向です．「速度」とは，エンコーダが連結されたモータ単体の出力軸上の回転角速度 ω [rad/s] となります．また，「加速度」とは，エンコーダが連結されたモータ単体の出力軸上の回転角加速度 ω' [rad/s²] となります．いずれも Movado の内部では「32 ビット固定小数点」によって表現されています．

inst=4 の場合，4 バイト引数（quad）を「副指令（sub-command）」と解釈し，それを実行します．副指令は，8 ビットの副命令（jnst），8 ビットのインデックス（index），16 ビットのワード（word）からなります．

なお副指令には，addr・n で指定されたモータを対象とする「モータ指令」（下表で「m」）と，addr で指定されたノードを対象とする「ノード指令」（下表で「N」）があります．ノード指令の場合，モータ番号 n は無視されます．

（１）パラメタを読み書きするための副指令群

Movado には，下表のように，各モータごとの位置・速度・加速度などの値を格納する 32 ビット固定小数点パラメタや，各モータごとの状態フラグなどの値を格納する 16 ビットパラメタがあります．

これらパラメタには，下表にあげる副指令をとおしてアクセスすることができます．set 系の命令（セルが赤）はパラメタの値を書き換えます．get 系の命令（表中でセルが青）はパラメタの値を読み出し，返信パケットを PC に送信します．

• set_upper 命令（jnst=0x00 かつ index<0xff）
  n によって指定されたモータについて，index で指定された 32 ビット固定小数点パラメタを上書きします．書き込まれるデータは，上部 16 ビットが upper となり，下部 16 ビットには set_lower 命令によって登録された下部バッファの値が入ります．
• set_lower 命令（jnst=0x00 かつ index=0xff）
  16 ビットデータ lower を下部バッファに書き込みます．この下部バッファは，すべてのモータ（n=0〜3）で共有されます．通常，まず set_lower 命令によって下位 16 ビットのデータを仮登録し，続いて set_upper 命令によって特定の index に対応する 32 ビット固定小数点パラメタを書き換えることになります．
• set_word 命令（jnst=0x01）
  n によって指定されたモータについて，index で指定された 16 ビットパラメタを，与えられた 16 ビットデータ word によって上書きします．
• get_upper 命令（jnst=0x80 かつ index<0xff）
  n によって指定されたモータについて，index で指定された 32 ビット固定小数点パラメタを読み出し，その上部 16 ビットを元のパケットの word 領域に格納して PC に返信するとともに，下部 16 ビットを下部バッファに書き込みます．この下部バッファは，すべてのモータ（n=0〜3）で共有されます．
• get_lower 命令（jnst=0x80 かつ index=0xff）
  下部バッファの値を元のパケットの word 領域に格納して PC に返信します．通常，まず get_upper 命令によって特定の index に対応する 32 ビット固定小数点パラメタの上位 16 ビットのデータを読み出し，続いて get_lower 命令によって下位 16 ビットを読み出します．
• get_word 命令（jnst=0x81）
  n によって指定されたモータについて，index で指定された 16 ビットパラメタを読み出し，その値を元のパケットの word 領域に格納して PC に返信します．

（２）動作キャリブレーションのための副指令群

Movado はインクリメンタル型エンコーダ付きのモータの制御を想定しています．このエンコーダでは，出力軸の絶対位置（絶対回転角度）を直接的に得ることができません．出力軸に取り付けられた機構（たとえばロボットの腕）を動かし，その物理的な限界点を見つけることで，Movado 内部での論理的な位置と Movado 外部での物理的な位置とをリンクさせる必要があります．下表は，そのために必要となる副指令をまとめたものです．

• set_zero 命令（jnst=0x7f かつ index=0x00）
  n で指定されたモータについて，現在の位置を原点（位置 0）とします．モータは動きません．
• do_max 命令（jnst=0x7f かつ index=0x05）
  n で指定されたモータについて，プラス（CW）方向の物理的な限界点まで回転させ，その限界点を 32 ビット固定小数点パラメタ MAX に記録します．set_vel によって設定された速度によってモータを等速回転させていき，モータの動きが emg [ms] 分以上の遅れを生じた場合，その位置を限界点とみなします．emg の値は試行錯誤をとおして見つけてください．多くの場合，emg の値として 100〜200 程度がよいでしょう．記録された限界点は，get_lower・get_upper 命令によって読み出すことができます．
• do_min 命令（jnst=0x7f かつ index=0x06）
  n で指定されたモータについて，マイナス（CCW）方向の物理的な限界点まで回転させ，その限界点を 32 ビット固定小数点パラメタ MIN に記録します．詳細は do_max の項目を参照してください．
• do_minmax 命令（jnst=0x7f かつ index=0x07）
  n で指定されたモータについて，最初に do_min を実行し，つぎに do_max を実行します．詳細は do_min・do_max の項目を参照してください．
• do_minmaxcen 命令（jnst=0x7f かつ index=0x10）
  n で指定されたモータについて，最初に do_minmax を実行し，つぎに得られた両限界点の中点 (MIN + MAX) / 2 に移動し，最後にその中点で set_zero を実行します．中点への移動には，set_vel によって設定された速度を使います． 詳細は do_minmax・set_zero の項目を参照してください．
• do_minmaxmax 命令（jnst=0x7f かつ index=0x11）
  n で指定されたモータについて，最初に do_minmax を実行し，つぎに得られたプラス方向の限界点 MAX で set_zero を実行します．詳細は do_minmax・set_zero の項目を参照してください．
• do_minmaxmin 命令（jnst=0x7f かつ index=0x12）
  n で指定されたモータについて，最初に do_minmax を実行し，つぎに得られたマイナス方向の限界点 MIN に移動し，最後にその限界点で set_zero を実行します．限界点への移動には，set_vel によって設定された速度を使います． 詳細は do_minmax・set_zero の項目を参照してください．

動作キャリブレーション命令（do_max, do_min など）の実行終了は，16 ビットパラメタ MMFLG を読み出すことで検出できます．MMFLG の値が 0 以外であれば動作キャリブレーションの実行中，値が 0 であれば動作キャリブレーションの終了を意味します．

（３）ラジコン用サーボを制御するための副指令群

Movado は最大 4 台のラジコン用サーボを制御することができます．制御のしかたは，DC モータの場合との同じように，台形速度制御（move_rcs）と最大パワー制御（setpos_rcs）を選べます．

ラジコン用サーボの出力軸は，位置 0x2400 [count] のときほぼ中央となります．およそ 60 [count/deg] の割合で，−60〜60 [deg] 程度の動作範囲をカバーすることができます．たとえば，出力軸の絶対位置を −30 [deg] にするには，位置 0x1cf8 [count] を指定します．なおラジコン用サーボは出力軸の絶対位置を直接制御するので，動作キャリブレーションは不要です．

• move_rcs 命令（jnst=0x70 かつ index=0x00）
  n によって指定されたラジコン用サーボ（RCSn）を，台形速度制御によって目標位置 pos_rcs まで回転させます．それ以降，ラジコン用サーボはその位置を保持するようになります．このときの加速度（の絶対値）は，後述する setacc_rcs 命令によって設定されたものが，また最高速度（の絶対値）は，後述する setvel_rcs 命令によって設定されたものが使われます．
• setacc_rcs 命令（jnst=0x70 かつ index=0x01）
  n で指定されたラジコン用サーボ（RCSn）の move_rcs 命令における加速度を acc_rcs に設定します．
• setvel_rcs 命令（jnst=0x70 かつ index=0x02）
  n で指定されたラジコン用サーボ（RCSn）の move_rcs 命令における最高速度を vel_rcs に設定します．
• setpos_rcs 命令（jnst=0x70 かつ index=0x03）
  n で指定されたラジコン用サーボ（RCSn）を，最大限のパワーで位置 pos_rcs まで回転させます．それ以降，ラジコン用サーボはその位置を保持するようになります．

（４）その他の副指令群（主なもの）

• reset 命令（jnst=0x7f かつ index=0xff）
  addr で指定されたノードについて，すべての 32 ビット固定小数点パラメタと 16 ビットパラメタを初期化します．
• get_report 命令（jnst=0xff）
  n で指定されたモータについて，interval [ms] ごとに count 回だけ，その各時点での現在の位置をパケット（getpos の返信パケットと同じ形式）の fix 領域に格納してをPC に返信します．

このページの先頭に戻る

move 系命令（move・movac・movel および move_rcs）では，与えられた加速度 acc [rad/s²]，最高速度 vel [rad/s] に従って，与えられた目標位置 pos [rad] までモータを回転させます．この制御方法は，一般に台形の速度プロファイルをもつようになるため，台形速度制御と呼ばれます．

move 系命令を受け取った Movado は，加速度 acc で加速し，最高速度 vel に達したら巡航状態に入り，適切なタイミングで加速度 −acc の減速に入り，目標位置 pos₁ で停止するというものです．その速度（上図）と位置（下図）の変化を模式的に表わすと，次のようになります．

たとえば，時刻 t₀ に move 系命令を受け取った場合，あらかじめ設定されていた（movac 命令の場合は明示的に与えられた）加速度 acc で加速しはじめます．これは等加速運動となるため，速度プロファイルは斜めの直線，位置プロファイルは放物線となります．

この速度の絶対値が，あらかじめ設定されていた（movel 命令の場合は明示的に与えられた）最高速度 vel に達した場合，それ以上の加速はぜずに，巡航状態（等速運動）に入ります．速度プロファイルは水平線，位置プロファイルは斜めの直線となります．なお，小さな加速度あるいは大きな最大速度が設定された場合，この巡航状態に達する前に（後述する）減速状態に入ることもあり，その場合の速度プロファイルは，台形ではなく二等辺三角形になります．

Movado は，一定の減速度（負の加速度 −acc）で目標位置 pos₁ で停止（速度を 0 に）できるように，減速を開始する時刻 t_dを計算します．つまり，速度プロファイルの台形（あるいは二等辺三角形）の面積が，目標位置までの移動距離（pos₁−pos₀）となるように，減速開始時刻を計算するわけです．この時刻 t_d に達したとき，Movado は減速を開始します．これは等減速運動となるため，速度プロファイルは斜めの直線，位置プロファイルは放物線となります．

最終的に，目標位置 pos₁ で停止します．計算上の誤差のため，一発で目標位置に停止できないことがありますが，その場合は自動的に微調整を行ない，最終的に目標位置 pos₁ で停止します．（ただし，計算上およびモータ制御特性上，わずかな誤差が生じることがあります.）

上の例では停止状態（速度 0）から台形速度制御を開始しましたが，そうでない場合でも move 系命令を実行することができます．たとえば，先行する move 系命令によって pos₁ に向けて移動している最中に，新しい move 系命令によって別の目標位置 pos₂ を与えることができます．すると Movado は，古い目標位置は忘れ，新しい目標位置 pos₂ に向かって，台形速度制御を実行するようになります．その速度（上図）と位置（下図）の変化を模式的に表わすと，たとえば次のようになります．

新しい目標位置を与えるとき，加速度や最高速度を変えることも自由にできます．ただし，大きな速度での移動中に小さな加速度 acc_n で新しい目標位置を与えた場合，いわばブレーキやアクセルの効きが悪い状態になるため，下図のように，目標位置に達するまで長い時間（と移動軌跡）が必要となります．この移動軌跡がモータ出力軸の動作範囲を越えてしまう可能性があるので，注意が必要です．

move 系命令を使って移動する場合，目標位置に達したことを検出したい場合があります．たとえば目標位置 pos₁ に移動し，その位置で何らかの仕事（たとえば測位や写真撮影）をしてから，つぎの目標位置 pos₂ に移動するような場合です．

このような場合は，get_word 命令を使って 16 ビットパラメタ CAMOD をチェックすることで解決できます．Movado は台形速度制御をしているあいだ，CAMOD に 0 以外の数値を格納します．台形速度制御が終了（目標位置でモータが停止）すると，Movado は CAMOD に 0 を格納します．

たとえば，目標位置 pos への move 系命令を送信したのち，この CAMOD パラメタを読み出す命令（sub/get_word）を送信し，その返信（パケットの word 部分に CAMOD の値が格納される）を受信することを 10〜100ms 程度の周期で繰り返します．CAMOD が 0 になった場合に繰り返しを終了すれば，確実に目標位置 pos が達成されたことを確認できます．

このページの先頭に戻る

Movado では，設定したアドレスによって，上表のエンコーダ分解能に適合したパラメタ（エンコーダ係数 ERPC8・速度フィルタ係数 LPFA）が自動設定されます．（4 つのモータすべてについて同じパラメタが自動設定されます.）

使用するエンコーダに合わせて，適切な範囲からアドレスを選択するとよいでしょう．また，これらパラメタを set_upper・set_lower 命令によって上書きすることで，表にない分解能のエンコーダに対応することや，ひとつの Pico-2 に分解能の異なる複数のエンコーダを接続することが可能になります．なお，ERPC8 は整数部 8 ビット・小数部 24 ビットの固定小数点形式をとるため，エンコーダ分解能（rad/count）の実数値を 2²⁴ 倍した値に設定してください．また，LPFA（32 ビット固定小数点形式）の値は，上表を参考にして，応答特性（とくにアンチエイリアシング効果の過不足）を見ながら決めてください．

Movado はモータを（巻線の焼損などから）保護するために，プラス方向・マイナス方向の物理的限界点から安全マージン（PMAR）だけ手前の位置に，動作限界点（MAX・MIN）を設定します．モータの目標位置 XPOS は，これら動作限界点の範囲（MIN ≤ XPOS ≤ MAX）に制限されます．

MAX の初期値は約 32768 [rad]（0x7fffffff），MIN の初期値は −32768 [rad]（0x80000000）となっています．動作キャリブレーション（do_max・do_min など）を実行することで，MAX・MIN の値は自動的に設定されます．また，PMAR の初期値は 1.0 [rad]（0x00010000）となっています．いずれのパラメタも set_upper・set_lower 命令によって任意の値に設定することもできます．

このページの先頭に戻る