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

前回CANopenのPDOを使って左右のモータードライバからエンコーダ情報を取得することができるようになりました。今回はPDOを使ってモーターを速度命令で回します。

以前の投稿でSDO（Service data object）を使って実現していましたが、今回はTPDOを使ってモーターを回します。

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

モーターを駆動するために次の事を行います。
1. モータードライバの状態遷移
2. 外部から速度命令の受信
3. TPDにて速度命令の送信

## 参考資料

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

|資料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情報|

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

## サンプルコード

作成したコードは下記githubにて公開しています。sample2ブランチです。

https://github.com/Panasonic-Advanced-Technology/CANopenLinux/tree/sample2

# モータードライバの状態遷移

## 状態遷移表

BLVD-KRDは資料3の5章にある状態遷移でコントロールします。状態の設定はControlword（6040h）、状態の取得はStatusword（6041h）で行います。

![DEV_state.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/48e2561e-a8ac-3362-54c0-590c9db1d823.png)

Controlwordでの状態遷移表は次の通りです。

![status_machine.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/c9a81f7b-e916-7096-ca30-671e5f229549.png)

controlwordのbit定義は次の通りです。

![6040.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/461dafd2-a2b5-1590-426b-aed05e68fd74.png)

ドライバから送信されるStatuswordは次のように定義されています。

![6041.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/8c4dcc50-d4cc-91d9-c3d3-6e4238ede7a5.png)

## CANopenNodeでの状態遷移実装

サンプル実装としては状態遷移は次のように実装しました。
- StatuswordをモーターPositionと共にSYNCイベントごとに受信する
- ユーザーからのモーター速度設定コマンド（追加実装）をによる速度変化をトリガとして、ControlwordとTarget velocityのTPDOを送信する

状態遷移はContorlwordを次のように設定することで行います。Operation enabled状態の時、速度命令によるモーター駆動が有効となります。

```plantuml
@startuml
state "Switch on disabled" as SOD
state "Ready to switch on" as RTSO
state "Switch on" as SO
state "Operation enabled" as OE

[*] -r-> SOD : 電源ON
SOD -r-> RTSO : Controlword\n0x6
RTSO -d-> SO : Controlword\n0x7
SO -l-> OE : Controlword\n0xf
```

### Statusword受信コールバックの実装

コールバック設定時、stream->objectにはcan idを設定したオブジェクトを設定しています。コールバック関数で参照することで左右どちらのモータードライバの情報か判断する事ができます。
受信したStatuswordはグローバル変数に格納します。

```C
/* 6041h statusword 受信のコールバック */
ODR_t H6041_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) {
        printf("H6041_write(%x, %x, %x)\n", stream, buf, countWritten);
        return ODR_DEV_INCOMPAT;
    }
    printf("H6041_write(%X) : ", *(uint16_t*)stream->object);

    uint16_t *can_id = (uint16_t*)stream->object;   /* can id の取得 */
    uint8_t idx;
    if (*can_id == NODE_ID_RIGHT)
        idx = IDX_RIGHT;
    else if (*can_id == NODE_ID_LEFT)
        idx = IDX_LEFT;
    else
    {
        printf("bad parameter\n");
        return ODR_DEV_INCOMPAT;
    }

    statusword_[idx] = *(STATUS_WORD_t*)buf;        /* Statuswordをグローバル変数に格納 */

    if (idx == IDX_RIGHT)
        printf("RIGHT = 0x%x ", statusword_[idx]);
    else
        printf("LEFT  = 0x%x ", statusword_[idx]);

    printf("RTSO(%d) SwOn(%d) OpeEn(%d) Fault(%d) SOD(%d) VE(%d)\n",
        statusword_[idx].RTSO, statusword_[idx].SO, statusword_[idx].OE, statusword_[idx].F, statusword_[idx].SOD, statusword_[idx].VE);

    return ODR_OK;
}
```

### Controlword送信コールバックの実装

後述するユーザからの速度命令によってTPDOの送信がトリガされると、送信前に送信コールバックが呼びだれます。送信コールバックでは状態に応じて送信バッファにデータを設定します。

```C
/* 6040h Controlwordのコールバック */
ODR_t H6040_read(OD_stream_t *stream, void *buf,
                   OD_size_t count, OD_size_t *countRead)
{
    if (stream == NULL || stream->object == NULL || buf == NULL || countRead == NULL) {
        printf("H6040_read(%x, %x, %x)\n", stream, buf, countRead);
        return ODR_DEV_INCOMPAT;
    }
    printf("H6040_read(%X): ", *(uint16_t*)stream->object);
    uint16_t *can_id = (uint16_t*)stream->object;               /* can idの取得 */
    uint8_t idx;
    if (*can_id == NODE_ID_RIGHT)
        idx = IDX_RIGHT;
    else if (*can_id == NODE_ID_LEFT)
        idx = IDX_LEFT;
    else
    {
        printf("bad parameter\n");
        return ODR_DEV_INCOMPAT;
    }

    // 現在の状態に応じて送信バッファにControlwordを設定する
    uint16_t* ctrl = (uint16_t*)buf;
    if (statusword_[idx].SO) {
        printf("Openation Enabled\n");
        *ctrl = 0xf;                                /* Switch on -> Operation enabled */
    } else if (statusword_[idx].RTSO) {
        printf("Send Switch On\n");
        *ctrl = 0x7;                                /* Ready to switch on -> Swtch on */
    } else if (statusword_[idx].SOD) {
        printf("Send Ready to switch on\n");
        *ctrl = 0x6;                                /* Switch on disabled -> Ready to swtich on */
    }

    return ODR_OK;
}
```

# 外部からの送信コマンドの受信

オブジェクトディクショナリに独自のエントリを加えることでCANopenNodeのユーザコマンドcocommを使って外部から速度パラメータを設定するようにします。

## オブジェクトディクショナリにエントリの追加

CANopenEditorを使い2000hにint32[2]のデータセットを追加します。右モータードライバのODにコントローラの情報も共有しているのでOD.cに2000hのエントリを次のように追加します。

- 「Object Dictionary」タグ→「Manifucture Specific Parameters」リストを右クリックしてエントリ追加を選択。「Velocity command」という名前をつけてエントリを追加。
- 画面右側の「Velocity command」を右クリックして値を追加する。オブジェクトタイプを"VAR"、データタイプを"INTEGER32"、Storage Groupを「RAM」とする。

![velocityのODエントリを追加.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/55d90631-aa5f-cbe8-2218-73cfee587da7.png)

追加ができたら、「File → Export CanOpenNode...」を選択してOD.hと名前を付けて保存する（右OD）。変更したOD.cをコンパイルするとcocommコマンドからODオブジェクト2000hに値を設定することができます。

```shell
$ cocomm "1 w 0x2000 1 i32 1"            # node id 1(コントローラ)の0x2000のsub index 1に1を設定
```

## 設定した値へのアクセス

cocommコマンドで設定した値はODオブジェクトのストレージRAMに格納されており次のグローバル変数でアクセスすることができます。

```C
OD_RAM.x2000_velocityCommand[];
```

cocommコマンドで設定された値を、アプリケーションのメインループから呼び出される関数app_programAsync()内で変化をチェックし、変化があれば取り込んだ上でControlwordのTPDOを送信するイベントをOD_requestTPDO()を呼び出してトリガする。

```C
void app_programAsync(CO_t *co, uint32_t timer1usDiff) {
    (void) co; (void) timer1usDiff; /* unused */
    bool val_chagned = false;

    if ( velocity_[IDX_RIGHT] != OD_RAM.x2000_velocityCommand[IDX_RIGHT])
    {
        velocity_[IDX_RIGHT] = OD_RAM.x2000_velocityCommand[IDX_RIGHT];
        printf("change right velocity. ReqestTPDO\n");
        /* Trigger TPDO */
        OD_requestTPDO(OD_targetVel_flagsPDO[IDX_RIGHT], 0);
        val_chagned = true;
    }

    if ( velocity_[IDX_LEFT] != OD_RAM.x2000_velocityCommand[IDX_LEFT])
    {
        velocity_[IDX_LEFT] = OD_RAM.x2000_velocityCommand[IDX_LEFT];
        printf("change left velocity. ReqestTPDO\n");
        /* Trigger TPDO */
        OD_requestTPDO(OD_targetVel_flagsPDO[IDX_LEFT], 0);
        val_chagned = true;
    }

    if (val_chagned)
        printf("left %d, right %d\n", velocity_[IDX_LEFT], velocity_[IDX_RIGHT]);
}
```

# TPDOで速度命令の送信

OD_requestTPDO()を呼び出すことで、同じTPDOにマップしているControlwordとTarget velocityが同じデータフレームで送信されます。送信前に、ControlwordとTarget velocityのコールバック関数が呼び出されますので、コールバック関数にて送信バッファに送信データを設定します。

Controlwordの設定は前述H6040_read()の通りです。

Target velocityは次のように実装しています。
```C
/* 60FFh Target velocityのコールバック */
ODR_t H60FF_read(OD_stream_t *stream, void *buf,
                   OD_size_t count, OD_size_t *countRead)
{
    if (stream == NULL || stream->object == NULL || buf == NULL || countRead == NULL) {
        return ODR_DEV_INCOMPAT;
    }
    printf("H60FF_read(%X) : ", *(uint16_t*)stream->object);
    uint16_t *can_id = (uint16_t*)stream->object;               /* can idの取得 */
    uint8_t idx;
    if (*can_id == NODE_ID_RIGHT)
        idx = IDX_RIGHT;
    else if (*can_id == NODE_ID_LEFT)
        idx = IDX_LEFT;
    else
    {
        printf("bad parameter\n");
        return ODR_DEV_INCOMPAT;
    }

    int32_t* ctrl = (int32_t*)buf;
    *ctrl = velocity_[idx];                 /* 送信バッファに速度指令値を書き込み */
    printf("set vel %d\n", *ctrl);

    return ODR_OK;
}
```

# 動作状況

terminal1: canopendの起動
```shell
$ ./canopend can0 -i 1 -c "local-/tmp/CO_command_socket" 
```

terminal2: cocommコマンドで通信状態をPreop→Operationalへ
```shell
$ ./cocomm "0 start"
```

terminal2: cocommコマンドで2000hへ速度コマンドを書き込んで動作状態を遷移させる
```shell
$ ./cocomm "1 w 0x2000 2 i32 1"
```
最後の"1"が速度値。canopend側では速度値が変化したときにControlwordを送信するようにしているので、値を変えて何度か送信することで、動作状態をOperaiton enabledにする。

![TPDO.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/5f17ed12-5bdd-f5e4-38bc-83cdc37cde8e.gif)

# 次回予定

今回でCANopenを用いてBLVD-KRDを速度命令で駆動し、角度情報を取得することができるようになりました。次回はCANopenNodeを用いて差動二輪制御によるROSロボットドライバを作成します。