実験用AGVの作成 - (4) - オリエンタルモーターをCANopenで回す(2)

前回、CANopenNodeのコマンドを使ってモーターを回してみました。今回は、CANOpenのPDOを使ったデータの受信を行います。

https://qiita.com/yamamoto-kazusige/private/e434fe5138860ce53d5d

## 参考資料

本記事ではオリエンタルモーターのホームページからダウンロードできる下記の資料を参照しています。

|資料No|ファイル名|タイトル|内容|
|---|---|---|--
|1|HP-5139J.pdf|BLVシリーズ Rタイプ 設置・接続編|モーターとドライバ、コントローラの接続方法について|
|2|HP-5141J.pdf|BLVシリーズ Rタイプ 機能編|
|3|HP-5143E.pdf|BLV Series R Type Driver CANopen Communication Profile|BLVD-KRDに関するObject Directory情報|

以下、説明内の引用資料は特に断りがない場合上記資料からの抜粋です。

# プロセスデータオブジェクト(PDO)

前回、オリエンタルモーターのBLVD-KRDをオープンソースのCANopenNodeを使って回してみました。前回の通信方法はリクエストを発行して通信を行うSDO(Service Data Object)を使って通信しました。

CANopenのデータ通信には幾つか種類があります。中でもSDOとPDO(Process Data Object)を用いて通信することが多いと思います。その他はエラー状態の通知、ノード間の同期に用いられます。

SDOはクライアント（コントローラ）からのアクセス要求によって発生します。前回デバイス状態の設定やモーター回転命令の設定に用いました。

```shell:モーターの位置情報取得
$ ./cocomm "10 r 0x6064 0 i32"
[1] 40819
```

モーター角度、センサー情報、駆動命令はリアルタイム性を求められます。このような場合はコントローラからの要求によって送受信されるのではなく、センサー側が自発的、同期的に配信する機能が必要です。PDOはこのような場合に用います。ノード毎に送信PDO(TPDO)、受信PDO(RPDO)を定義できます。

# BLVD-KRDのTPDO

MEXE02の「CANopen通信ステータスモニタ」画面でBLVD-KRDに設定されているPDOを確認できます。デフォルトでは４つあるマップの1と2に状態設定/ステータス応答がマップされており、マップ３に位置設定コマンド、マップ４に速度設定コマンドが設定されています。

![PDOmap.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/d9a366be-c925-05ea-100d-1f0cacec4823.png) [MEXE02のスクリーンショット]

作動二輪駆動ドライバを構成する際、モータードライバの状態と現在の回転角を必要としますので、TPDOマップ３の情報（6041h:statusword、6064h:Position actual value）を取得できればよいです。

## コマンドにより3rdTPDOを設定する

3rd TPDOの設定は1802hで行います。

![1803h.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/99cd0d2d-9eeb-6a32-4412-45f8943580a5.png)

Sub-index 02h(Transmission type)を0xff(event-driven), Sub-index 03h(Inhabit time)を 10000(x100μsec), Sub-index 0x05h(Event timer)を 1000(ms)に設定します。

```shell
$ cocomm "set node 10"
$ cocomm "w 0x1802 2 x8 0xff" "w 0x1802 3 u16 10000" "w 0x1802 5 u16 1000"
$ cocomm "start"
```
candumpで通信を確認すると次のように１秒周期でモーター位置が配信されていることが確認できます。

```shell
$ candump -x -td can0
 (000.999935)  can0  RX - -  38A   [6]  60 12 E9 05 00 00
 (000.999988)  can0  RX - -  38A   [6]  60 12 E8 05 00 00
 (000.999907)  can0  RX - -  38A   [6]  60 12 EA 05 00 00
 (001.000029)  can0  RX - -  38A   [6]  60 12 EC 05 00 00
```

イベントの配信はSub-index 05hに設定した時間間隔経過するか、データに変化があったときです。モーター回転角データはノイズにより静止時に変動があるため、Sub-index 03hのInhabit time(配信禁止時間)を設定しないと非常に高レートでデータが配信されます。

## MEXE02でTPDOを設定する

上記ではBLVD-KRD起動後にコマンドでTPDO配信の設定を行いました。MEXE02を使ってBLVD-KRDのデフォルト設定を書き換えると、通信状態が"Operational"になったときに自動的にデータを配信するようになります。

MEXE02の「CANopen object通信初期値・設定」画面から設定値を変更した後「データ書き込み」を実行してBLVD-KRDの不揮発メモリに設定を書き込みます。

# CANopenNodeのPDO設定

BLVD-KRDから配信されるデータをコントローラ(CANopenNode)側で受信するには、CANopenNode側のRPDOの設定(160xh)を行いますが、受信したときのハンドラ関数の定義などの必要ですので、CANopenNodeのコードの変更を行います。

## CANopenEditor

CANopenNodeには、Object Directoryの設定ファイルを編集するアプリケーション"CANopenEditor"が併せて配布されています。

https://github.com/CANopenNode/CANopenEditor

ODの定義ファイル(.eds)を読み込んで、必要なエントリを編集する機能と定義情報を出力(.c/.h形式)する機能を備えています。

ビルド済みのexeファイルはbuildブランチから取得できます。

## DS301プロファイルにPDOエントリを追加する

CANopenNodeに含まれるOD定義ファイル```OD.c```には1000h番台のオブジェクトしか定義されていないため、デバイス制御オブジェクトを追加する必要があります。

- ```EDSEditor.exe```を起動し、"File"メニュー→"Open"を選択してオリエンタルモーターのホームページからダウンロードしたBLVD-KRD用のEDSファイル(BLVD-KRD_CANopen_V200.eds)を開く
- 「Object Directory」タブを開き、1000h番台の「Communication Specific Parameters」の全ての項目を選択してから、マウスの右クリックでメニューから削除を選択する
- "Insert Profile"メニューから"DS301_profile.xpd"を選択。すべての項目にチェックマークをつけて"Insert"ボタンを押下する
- "File"メニューから"Export CanOpenNode..."を選択する。

これで、BLVD-KRDのODを定義したテンプレートが生成されます(OD.cとOD.h)。

## サンプルプログラムに組み込む

CANopenDemo/CANopenLinux/CANopenNode/example以下のOD.cとOD.hを上記で保存したテンプレートと置き換えます。

BLVD-KRDから配信されるTPDOをコントローラプログラムのRPDO 1stで受けるようにOD.cを修正します。

```c:変更前
    .x1400_RPDOCommunicationParameter = {
        .highestSub_indexSupported = 0x05,
        .COB_IDUsedByRPDO = 0x80000200,
        .transmissionType = 0xFE,
        .eventTimer = 0x0000
    },
```
の、COB_IDUsedByRPDOを0x80000200→0x0000038Aに変更します。
```c:変更後
    .x1400_RPDOCommunicationParameter = {
        .highestSub_indexSupported = 0x05,
        .COB_IDUsedByRPDO = 0x0000038A,
        .transmissionType = 0xFE,
        .eventTimer = 0x0000
    },
```

受信するデータは6041hのstatusworldと6064hのPosition actual valueなので、x1600_RPDOMappingParameter を次のように書き換えます。
```c:変更前
    .x1600_RPDOMappingParameter = {
        .numberOfMappedApplicationObjectsInPDO = 0x00,
        .applicationObject1 = 0x00000000,
        .applicationObject2 = 0x00000000,
        .applicationObject3 = 0x00000000,
        .applicationObject4 = 0x00000000,
        .applicationObject5 = 0x00000000,
        .applicationObject6 = 0x00000000,
        .applicationObject7 = 0x00000000,
        .applicationObject8 = 0x00000000
    },
```
```c:変更後
    .x1600_RPDOMappingParameter = {
        .numberOfMappedApplicationObjectsInPDO = 0x02,
        .applicationObject1 = 0x60410010,
        .applicationObject2 = 0x60640020,
        .applicationObject3 = 0x00000000,
        .applicationObject4 = 0x00000000,
        .applicationObject5 = 0x00000000,
        .applicationObject6 = 0x00000000,
        .applicationObject7 = 0x00000000,
        .applicationObject8 = 0x00000000
    },
```
また、ODの設定により特定のオブジェクトを受信したときに呼び出すハンドラを指定することができます。ハンドラの定義はOD.cで定義されているエントリ配列
```ODList[]```で設定します。

```c
static OD_ATTR_OD OD_entry_t ODList[] = {
    
    {0x6064, 0x01, ODT_VAR, &ODObjs.o_6064_positionActualValue, NULL},
    
};
```
OD_entry_tの５つめのメンバがハンドラ定義のメンバで、デフォルトではNULLに定義されていてハンドラは呼び出されません。

また、このエントリへのアクセスはOD.hでマクロ定義されています。
例えば、6064hのPosition actual valueにアクセスするエントリは次のように定義されています。
```c
#define OD_ENTRY_H6064 &OD->list[122]
```

## ハンドラ呼び出し

ハンドラの呼び出し部分のサンプルは ```CANopenDemo/demo/CO_application.c``` にあります。
サンプルはデモデバイス向けの設定なので、app_programStart()での処理を次のように変更します。

```c++:CO_application.c

ODR_t H6064_write(OD_stream_t *stream, const void *buf,
                   OD_size_t count, OD_size_t *countWritten)
{

    if (stream == NULL || stream->object == NULL || buf == NULL || countWritten == NULL) {
        return ODR_DEV_INCOMPAT;
    }

    int32_t pos = *(int32_t*)buf;
    printf("H6064_write : pos = %d\n", pos);
    return ODR_OK;
}

OD_extension_t H6064_extention = {
    .object = NULL,
    .read = NULL,
    .write = H6064_write,
};

/******************************************************************************/
CO_ReturnError_t app_programStart(uint16_t *bitRate,
                                  uint8_t *nodeId,
                                  uint32_t *errInfo)
{
    CO_ReturnError_t err = CO_ERROR_NO;

    OD_extension_init(OD_ENTRY_H6064_positionActualValue, &H6064_extention);

    return err;
}
```

プログラムを起動しBLVD-KRDをONにするとモーター角を受信するたびに下記のような出力が行われます。

```
H6064_write : pos = 42895
H6064_write : pos = 42894
H6064_write : pos = 42998
```

モーターポジションの解像度は１回転あたり36,000カウントと高精度なので動いていない状態でも多少の変動があります。

## 複数デバイスへの対応

作動二輪駆動の車両を構成する場合、左右２つのモーターを制御する必要があります。ODへのコールバックハンドラの登録でモーターポジションを取得することができましたが、このデータが左右どちらのモーターから来たデータか区別することができません。

CANopenNodeの開発者に問い合わせてみましたが、RPDOのコールバックにはデバイスを識別するIDは入ってこないのがプロトコルの仕様であり、デバイスを区別する場合はそれぞれに異なるオブジェクトディクショナリを定義して、異なるコールバックを設定して下さい。という回答でした。
CANopenNodeにはこの実装に対応するため```CO_MULTIPLE_OD```というdefineがあります。しかし。。。exampleには
```c
/* example usage of CO_MULTIPLE_OD (but still single OD here) */
```
単一のODの例しかありません^^;。仕方ないのでCANopenNodeのソースコードを読んで使い方を解いてみました。次回複数ODの実装について書く予定です。


https://qiita.com/yamamoto-kazusige/items/2e14a164184a2e67742b