ジオニックテクニクスで使用しているサーボモータのKRS-3204と、KRS-3302をシリアルコントロールする話です。
通信インターフェースのICSに対応したIPが落ちていないのでこれを自作していきます。
せっかくの題材なのでこれまであまり本格的にやっていなかった高位合成を試していくこととします。
とりあえずは仕様を検討していきます。
ICSの仕様は
ここです。日本語なので楽勝。必要そうな情報は以下の通り。
最大接続数：
Max 32個(ID:0〜31)
定周期送信データ：
ポジション設定 14bit(周期0)x32
定周期受信データ：
現在ポジション 14bit(周期0)x32
現在電流値 7bit(周期1)x32
現在温度 7bit(周期2)x32
オンデマンド送信データ：
ストレッチ 7bit
スピード 7bit
電流制限 7bit
温度制限 7bit
EEPROM 64byte
オンデマンド受信データ：
EEPROM 64byte
(角度取得 14bit => ソフトで初期処理時に定周期送信データのポジション設定に反映する)
ポジションと電流値は制御に使いそうなので早い周期で回しておきます。
温度は比較的に急変しにくいと思うので少しゆっくりした周期で取得します。
オンデマンドの送信はそれほど多く発生しないと思われるので、全体で共有します。
ROSのサービスで使われることを想定します。
設定が反映されているかどうかはオンデマンドでEEPROMのデータを取得して確認します。
角度取得は初期化時にオンデマンドで取得できるようにしておきます。
定周期送信データは32ch分を一括で転送できるように連続して配置します。
定周期受信データも一括して取得できるようにポジション、電流値、温度毎に連続して配置します。
とりあえずレジスタマップはこんな感じかな。
| offset | name | bit | R/W | initial value | description |
| 0x0010 | baud_rate | [3:0] | R/W | 0b0000 | 0:115200bps
1:625000bps
2:1152000bps
other:禁止 |
|
| [31:4] | R | 0x0000000 | RSV |
| 0x0018 | number_of_servos | [7:0] | R/W | 0x01 | ICSで通信するサーボの数を書くこと |
|
| [31:8] | R | 0x000000 | RSV |
| 0x0020 | cyclic0_config | [15:0] | R/W | 0x000A | cyclic0で通信する周期をms単位で指定 |
|
| [30:16] | R | 0x0000 | RSV |
|
| [31] | R/W | 0b0 | 0:cyclic0の通信停止
1:cyclic0の通信開始 |
| 0x0028 | cyclic1_config | [15:0] | R/W | 0x000A | cyclic1で通信する周期をms単位で指定 |
|
| [30:16] | R | 0x0000 | RSV |
|
| [31] | R/W | 0b0 | 0:cyclic1の通信停止
1:cyclic1の通信開始 |
| 0x0030 | cyclic2_config | [15:0] | R/W | 0x03E8 | cyclic2で通信する周期をms単位で指定 |
|
| [30:16] | R | 0x0000 | RSV |
|
| [31] | R/W | 0b0 | 0:cyclic2の通信停止
1:cyclic2の通信開始 |
| 0x0038 | ondemand_cmd | [7:0] | R/W | 0x00 | 送信サイズをバイト単位で指定 |
|
| [30:8] | R | 0x00 | RSV |
|
| [31] | R | 0b0 | ondemand通信ステータス
0:通信完了
1:通信中 |
|
| [31] | W | 0b0 | ondemand通信コマンド
0:変化なし
1:通信指示 |
| 0x0040 | RSV | [31:0] | R/W | 0x00000000 | RSV |
| ︙ |
|
|
|
|
|
| 0x07FC | RSV | [31:0] | R/W | 0x00000000 | RSV |
| 0x0800 | cyclic0_tx_0 | [7:5] | R/W | 0b100 | ポジション設定コマンド(0b100)を書くこと |
|
| [4:0] | R/W | 0b00000 | 対象となるサーボのIDを書くこと |
|
| [15:8] | R/W | 0x00 | RSV |
|
| [31:16] | R/W | 0x0000 | 設定舵角14bit(bit31,bit30は0固定) |
| 0x0804 | cyclic0_tx_1 | [31:0] | R/W | 0x00000080 | cyclic0_tx_0同様 |
| 0x0808 | cyclic0_tx_2 | [31:0] | R/W | 0x00000080 | cyclic0_tx_0同様 |
| ︙ |
|
|
|
|
|
| 0x087C | cyclic0_tx_31 | [31:0] | R/W | 0x00000080 | cyclic0_tx_0同様 |
| 0x0880 | cyclic1_tx_0 | [7:5] | R/W | 0b101 | 15 |
|
| [4:0] | R/W | 0b00000 | 対象となるサーボのIDを書くこと |
|
| [15:8] | R/W | 0x03 | サブコマンドとして電流値を指定すること(0x03) |
|
| [23:16] | R/W | 0x00 | RSV |
|
| [31:24] | R/W | 0x00 | RSV |
| 0x0884 | cyclic1_tx_1 | [31:0] | R/W | 0x000003A0 | cyclic1_tx_0同様 |
| ︙ |
|
|
|
|
|
| 0x08FC | cyclic1_tx_31 | [31:0] | R/W | 0x000003A0 | cyclic1_tx_0同様 |
| 0x0900 | cyclic2_tx_0 | [7:5] | R/W | 0b101 | パラメータ読み出しコマンド(0b101)を書くこと |
|
| [4:0] | R/W | 0b00000 |
|
|
| [15:8] | R/W | 0x04 | サブコマンドとして温度を指定すること(0x04) |
|
| [23:16] | R/W | 0x00 | RSV |
|
| [31:24] | R/W | 0x00 | RSV |
| 0x0904 | cyclic2_tx_1 | [31:0] | R/W | 0x000004A0 | cyclic2_tx_0同様 |
| ︙ |
|
|
|
|
|
| 0x097C | cyclic2_tx_31 | [31:0] | R/W | 0x000004A0 | cyclic2_tx_0同様 |
| 0x0980 | ondemand_tx_0_wd0 | [7:5] | R/W | 0b000 | ICSの任意のコマンドを指定すること |
|
| [4:0] | R/W | 0b00000 | 対象のIDを記載すること |
|
| [15:8] | R/W | 0x00 | ICSの2byte目を書くこと |
|
| [23:16] | R/W | 0x00 | ICSの3byte目を書くこと |
|
| [31:24] | R/W | 0x00 | ICSの4byte目を書くこと |
| 0x0984 | ondemand_tx_0_wd1 | [7:0] | R/W | 0x00 | ICSの5byte目を書くこと |
|
| [15:8] | R/W | 0x00 | ICSの6byte目を書くこと |
|
| [23:16] | R/W | 0x00 | ICSの7byte目を書くこと |
|
| [31:24] | R/W | 0x00 | ICSの8byte目を書くこと |
| ︙ |
|
|
|
|
|
| 0x09C0 | ondemand_tx_0_wd17 | [7:0] | R/W | 0x00 | ICSの65byte目を書くこと |
|
| [15:8] | R/W | 0x00 | ICSの66byte目を書くこと |
|
| [23:16] | R/W | 0x00 | RSV |
|
| [31:24] | R/W | 0x00 | RSV |
| 0x09C4 | RSV | [31:0] | R/W | 0x00 | RSV |
| ︙ |
|
|
|
|
|
| 0x0C00 | cyclic0_rx_0 | [7:5] | R/W | 0x0000 | cyclic0_tx_0に対応する返信コマンド(0b000) |
|
| [4:0] | R/W | 0b00 | cyclic0_tx_0に対応する返信データに記載されているサーボID |
|
| [15:8] | R/W | 0x00 | 受信カウンタ |
|
| [31:16] | R/W | 0x00 | 現在角度の取得結果。通信データと異なり14bitのデータに復元する |
| 0x0C04 | cyclic0_rx_1 | [31:0] | R/W | 0x00000000 | cyclic0_rx_0同様 |
| ︙ |
|
|
|
|
|
| 0x0C7C | cyclic0_rx_31 | [31:0] | R/W | 0x00000000 | cyclic0_rx_0同様 |
| 0x0C80 | cyclic1_rx_0 | [7:0] | R/W | 0x00 | cyclic1_tx_0に対応した返信コマンドとIDが保存される |
|
| [15:8] | R/W | 0x00 | cyclic1_tx_0に対応したサブコマンドが保存される |
|
| [23:16] | R/W | 0x00 | 読み出した電流値が保存される |
|
| [31:24] | R/W | 0x00 | 受信カウンタ |
| 0x0C84 | cyclic1_rx_1 | [31:0] | R/W | 0x00000000 | cyclic1_rx_0同様 |
| ︙ |
|
|
|
|
|
| 0x0CFC | cyclic1_rx_31 | [31:0] | R/W | 0x00000000 | cyclic1_rx_0同様 |
| 0x0D00 | cyclic2_rx_0 | [7:0] | R/W | 0x00 | cyclic1_tx_0に対応した返信コマンドとIDが保存される |
|
| [15:8] | R/W | 0x00 | cyclic1_tx_0に対応したサブコマンドが保存される |
|
| [23:16] | R/W | 0x00 | 読み出した温度値が保存される |
|
| [31:24] | R/W | 0x00 | 受信カウンタ |
| 0x0D04 | cyclic2_rx_1 | [31:0] | R/W | 0x00000000 | cyclic2_rx_0同様 |
| ︙ |
|
|
|
|
|
| 0x0D7C | cyclic2_rx_31 | [31:0] | R/W | 0x00000000 | cyclic2_rx_0同様 |
| 0x0D80 | ondemand_rx_0_wd0 | [7:5] | R/W | 0b000 | ondemand_tx_0_wd0に対応した返信コマンドが保存される |
|
| [4:0] | R/W | 0b00000 | ondemand_tx_0_wd0に対応した返信コマンドのIDが保存される |
|
| [15:8] | R/W | 0x00 | ondemand_tx_0の返信データ2byte目が保存される |
|
| [23:16] | R/W | 0x00 | ondemand_tx_0の返信データ3byte目が保存される |
|
| [31:24] | R/W | 0x00 | ondemand_tx_0の返信データ4byte目が保存される |
| ︙ |
|
|
|
|
|
| 0x0DC0 | ondemand_rx_0_wd17 | [7:0] | R/W | 0x00 | ondemand_tx_0の返信データ65byte目が保存される |
|
| [15:8] | R/W | 0x00 | ondemand_tx_0の返信データ66byte目が保存される |
|
| [23:16] | R/W | 0x00 | RSV |
|
| [31:24] | R/W | 0x00 | 受信カウンタ |
| 0x0DC4 | RSV | [31:0] | R/W | 0x00000000 | RSV |
| ︙ |
|
|
|
|
|
| 0x0FFC | RSV | [31:0] | R/W | 0x00000000 | RSV |
