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

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

モーターを駆動するために次の事を行います。

モータードライバの状態遷移
外部から速度命令の受信
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ブランチです。

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

状態遷移表

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

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

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

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

CANopenNodeでの状態遷移実装

サンプル実装としては状態遷移は次のように実装しました。

StatuswordをモーターPositionと共にSYNCイベントごとに受信する
ユーザーからのモーター速度設定コマンド（追加実装）をによる速度変化をトリガとして、ControlwordとTarget velocityのTPDOを送信する

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

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

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

/* 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の送信がトリガされると、送信前に送信コールバックが呼びだれます。送信コールバックでは状態に応じて送信バッファにデータを設定します。

/* 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」とする。

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

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

設定した値へのアクセス

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

OD_RAM.x2000_velocityCommand[];

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

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は次のように実装しています。

/* 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の起動

$ ./canopend can0 -i 1 -c "local-/tmp/CO_command_socket"

terminal2: cocommコマンドで通信状態をPreop→Operationalへ

$ ./cocomm "0 start"

terminal2: cocommコマンドで2000hへ速度コマンドを書き込んで動作状態を遷移させる

$ ./cocomm "1 w 0x2000 2 i32 1"

最後の"1"が速度値。canopend側では速度値が変化したときにControlwordを送信するようにしているので、値を変えて何度か送信することで、動作状態をOperaiton enabledにする。

次回予定

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