LoginSignup
3
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@yamamoto-kazusige(山本 和成)inパナソニック アドバンストテクノロジー株式会社

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

Last updated at Posted at 2023-08-29

本業プロジェクトのソフトリリースのために前回の投稿から少し時間が空いてしまいました。

前回PDOを使ってモーターのエンコーダー情報の取得を行いました。今回は差動二輪制御AGVの左右２つのモーターの情報を取得します。CANopenNodeには複数のObjectDirectory情報を扱うための定義CO_MULTIPLE_OD がありますが実装例がありません。
CANopenNodeを使ってCAN上に接続された2つのモータードライバ（BLVD-KRD）からエンコーダ情報を取得するため、下記の２つのことに取組みました。

  1. SYNCイベントに応じてモーターエンコーダ情報を受信する
    前回はモータードライバ側のタイマイベントにより定期的にデータを送信するようにしていました。この設定ではモータードライバからデータ送信タイミングの同期が取れていません。CAN OpenではSYNCイベントという複数のデバイスの同期を取るための機能があります。
  2. 左右２つのモーターのエンコーダ情報を受信する
    SYNCイベント送信に応答してBLVD-KRDから送られるエンコーダ情報を受信します。この時左右のドライバをCAN IDにより識別します。

参考資料

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

資料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 Dictionary情報
4 https://github.com/CANopenNode CANopenNode is free and open source CANopen protocol stack. Apache-2.0 license

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

サンプルコード

作成したコードは下記githubにて公開しています。

SYNCイベントの設定

前回はユーティリティソフトMEXE02を使って、BLVD-KRDが自発的にエンコーダ情報を送信するように設定しました。CAN OpenではSYNCプロデューサーが送信したSYNCオブジェクトに応答する形でPDOオブジェクトを送信することで複数のデバイスの同期を取ることができます。

sync.png

モータードライバの設定

MEX02を使ってBLVD-KRDの1803hのパラメータをSYNCに同期するように書き換えます。

1803h.png

Sub-index 02h(Transmission type)を0x01(synchronous (cyclic every SYNC))に設定します。この設定により、モータードライバはSYNCオブジェクトを受信する毎にTPDOを送信します。TPDOのマッピング設定は前回と同様です。

CANopenNodeの設定(cocommコマンドによる設定)

SYNCの設定

SYNCオブジェクトのプロデューサー（送信）側は、CANopneNodeです。CANopenNode起動後にcocommコマンドを使って1006h(Communication cycle period)にイベントサイクルを設定した後、1005h(COB-ID SYNC message)をenabledにします。CANopenNodeのnode idは1。

canopendの起動
$ ./canopend can0 -i 1 -c "local-/tmp/CO_command_socket"
SYNCのサイクルを100,000μsecに設定
$ ./cocomm "1 w 0x1006 0 u32 100000"

1006h.png

SYNCメッセージを有効にする
$ ./cocomm "1 w 0x1005 0 x32 0x40000080"

1005h.png

同期開始

ネットワークステータスをOperationalにします。

$ ./cocomm "0 start"

candumpを確認すると下記のようにモータードライバからエンコーダ情報がバスに流れます。

$ candump -x -ta can0
 (1692077030.539135)  can0  TX - -  080   [0]                      <- SYNC
 (1692077030.540995)  can0  RX - -  38A   [6]  60 12 31 FA FF FF   -> node id 10からのstatus&encoder情報
 (1692077030.540997)  can0  RX - -  38B   [6]  60 12 7D 1B 02 00   -> node id 11からのstatus&encoder情報
 (1692077030.639387)  can0  TX - -  080   [0]
 (1692077030.641247)  can0  RX - -  38A   [6]  60 12 30 FA FF FF
 (1692077030.641249)  can0  RX - -  38B   [6]  60 12 7C 1B 02 00
 (1692077030.739667)  can0  TX - -  080   [0]
 (1692077030.741494)  can0  RX - -  38A   [6]  60 12 2C FA FF FF
 (1692077030.741495)  can0  RX - -  38B   [6]  60 12 7B 1B 02 00

順番があべこべですが、既にバス上にモータードライバを2つ接続しており、それぞれのnode idを10と11に設定します（COB-IDは38Ahと38Bh）。なので、プロデューサー(CANopenNode)からのSYNCに応答して、それぞれのドライバから応答が帰ってきています。

SYNCの送出間隔も設定の100msec周期になっています。

 (1692077030.539135)  can0  TX - -  080   [0] 
 (1692077030.639387)  can0  TX - -  080   [0]
 (1692077030.739667)  can0  TX - -  080   [0]

CANopenNodeの設定(プログラムからの設定)

次に、CANopenNodeのAPIを使って同期設定を行います。前回の投稿にて組み込んだサンプルプログラムの初期化関数app_prgramStart()に設定を組み込みます。

CO_application.c
CO_ReturnError_t app_programStart(uint16_t *bitRate,
                                  uint8_t *nodeId,
                                  uint32_t *errInfo)
{
    ・・・
    //*************************************
    // set sync
    //*************************************
    ODR_t odRet;
    // Set SYNC Period
    odRet = OD_set_u32(OD_ENTRY_H1006_communicationCyclePeriod, 0, 100000, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_ENTRY_H1006_communicationCyclePeriod);
        return CO_ERROR_OD_PARAMETERS;
    }
    // Enable SYNC
    odRet = OD_set_u32(OD_ENTRY_H1005_COB_ID_SYNCMessage, 0, 0x40000080, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_ENTRY_H1005_COB_ID_SYNCMessage);
        return CO_ERROR_OD_PARAMETERS;
    }
    ・・・
}

canopendを起動後、ネットワークステータスをOperationalにすると、エンコーダ情報が流れてきます。

左右のモータードライバからエンコーダ情報情報をRPDOで受信する

前回RPDOを使ってモータードライバからエンコーダ情報を受信しました。CANopenNodeを使ってRPDOを受信したとき、呼び出されるコールバック関数ではデータの送信元デバイスのnode idなどデバイスを区別する方法がありません。

CANopenNodeのissuesで問い合わせたところ「それがCANopenの仕様なので、送信元デバイスを区別する方法はない。それぞれ別のオブジェクトディクショナリ（OD）を定義して受信してね」という事らしい。そしてCANopenNodeのサンプルプログラムには、ODが1つのサンプルしかなくQAには「複数のODを扱うのはケースによって色々あるからサンプル提供は難しい」という過去のやり取りが＿|￣|○。というところまでが前回。

CANopenNodeのソースを読んで複数のデバイスから同じオブジェクト情報（今回はエンコーダ情報 6064h: Position actual value）を受信するために下記のことを行いました。

  1. CANopenEditorを使って2つ目のデバイスのODファイルOD_2nd.cを生成
  2. CANopenNodeのコンパイルオプションにCO_MULTIPLE_ODのdefineを追加
  3. canopendの初期化時に、新たに定義したODの初期化処理を追加
  4. サンプルプログラムの初期化app_programStart()でそれぞれのODの設定を行う。この時コールバックハンドラにデバイスを区別する情報を渡す

今回右のモータドライバのnode idを0xA、左のモータドライバのnode idを0xBとしています。

2つ目のオブジェクトディクショナリを定義

CANopenEditorを起動し、2つ目のODに必要なエントリを追加します。2つ目の方は必要なエントリだけ追加しました。本来は、デバイスを管理するノード(PC側)、左右のドライバを分離して3つのODを定義するのが正しい気がしますが、今回はとりあえず1つめのODファイルに通信パラメータを含めて、2つ目のODファイルにはPDO関連のエントリだけ追加しました。

  1. 「File → Open」 BLVD-KRD_CANopen_V200.edsを開く
  2. 「Object Directory」タグから、Communication Specific Parametersのすべてのオブジェクトを選択して右クリックっで削除
  3. 「Insert Profile → DS301_profile.xpd」を選択しそのまま「Insert」
  4. 「File → Export CanOpenNode...」を選択してOD.hと名前を付けて保存する（右OD）
  5. 「Object Directory」タグCommunication Specific Parametersの0x1000～0x1400のオブジェクトを選択して右クリックで削除
  6. 「File → Export CanOpenNode...」を選択してOD_2nd.hと名前を付けて保存する（左OD）

CANopenNodeのコンパイラオプションの変更

canopendのMakefileを修正し、OD_2nd.cの追加とコンパイルオプションを変更します。

Makefile
SOURCES = \
        $(DRV_SRC)/CO_driver.c \
        $(DRV_SRC)/CO_error.c \
        $(DRV_SRC)/CO_epoll_interface.c \
        $(CANOPEN_SRC)/301/CO_ODinterface.c \
        $(CANOPEN_SRC)/301/CO_NMT_Heartbeat.c \
        $(CANOPEN_SRC)/301/CO_Emergency.c \
        $(CANOPEN_SRC)/301/CO_SDOserver.c \
        $(CANOPEN_SRC)/301/CO_SDOclient.c \
        $(CANOPEN_SRC)/301/CO_TIME.c \
        $(CANOPEN_SRC)/301/CO_SYNC.c \
        $(CANOPEN_SRC)/301/CO_PDO.c \
        $(CANOPEN_SRC)/301/crc16-ccitt.c \
        $(CANOPEN_SRC)/301/CO_fifo.c \
        $(CANOPEN_SRC)/303/CO_LEDs.c \
        $(CANOPEN_SRC)/304/CO_GFC.c \
        $(CANOPEN_SRC)/304/CO_SRDO.c \
        $(CANOPEN_SRC)/305/CO_LSSslave.c \
        $(CANOPEN_SRC)/305/CO_LSSmaster.c \
        $(CANOPEN_SRC)/309/CO_gateway_ascii.c \
        $(CANOPEN_SRC)/storage/CO_storage.c \
        $(CANOPEN_SRC)/extra/CO_trace.c \
        $(CANOPEN_SRC)/CANopen.c \
        $(APPL_SRC)/OD.c \
        $(APPL_SRC)/OD_2nd.c \               /* 追加 */
        $(DRV_SRC)/CO_main_basic.c \
        $(DRV_SRC)/CO_application.c
Makefile
OPT += -DCO_MULTIPLE_OD                      /* 追加 */

2つ目のオブジェクトディクショナリ初期化処理の追加

canopendのmain()にOD_2ndの初期化処理を追加します。修正はgithubのsample1ブランチを参照してください。修正は次の通り。

  1. OD_2nd.hのinclude
  2. global変数CO_2ndの追加とオブジェクト生成
  3. CO_2ndをdisableにしてからCAN初期化。node idによるパケットのフィルタリング設定等
  4. CO_2ndのCANOpen初期化。PDOのコールバックハンドラなどの初期化
  5. イベントループでCO_2ndの処理追加

コールバックハンドラの設定

CO_application.cのapp_programStart()を修正し、PDOのコールバックハンドラを登録します。

CO_application.c
/* 6064h エンコーダ情報受信コールバックハンドラ */
ODR_t H6064_write(OD_stream_t *stream, const void *buf,
                   OD_size_t count, OD_size_t *countWritten)
{
    printf("H6064_write");
    if (stream == NULL || stream->object == NULL || buf == NULL || countWritten == NULL) {
        printf("\n");
        return ODR_DEV_INCOMPAT;
    }
    // stream->objectには、app_programStart()でハンドラに登録した
    // オブジェクトが設定されている
    uint16_t *can_id = (uint16_t*)stream->object;
    int32_t pos = *(int32_t*)buf;
    if (can_id && *can_id == 0x038A)
        printf("  posR = %d\n", pos);
    else if (can_id && *can_id == 0x038B)
        printf("  posL = %d\n", pos);
    else
        printf("bad parameter\n");

    return ODR_OK;
}

// 右のハンドラ情報
OD_extension_t H6041_extentionR = {
    .object = NULL,
    .read = NULL,
    .write = H6041_write,
};
// 左のハンドラ情報
OD_extension_t H6041_extentionL = {
    .object = NULL,
    .read = NULL,
    .write = H6041_write,
};

/** アプリケーション初期化関数 **/
CO_ReturnError_t app_programStart(uint16_t *bitRate,
                                  uint8_t *nodeId,
                                  uint32_t *errInfo)
{
    CO_ReturnError_t err = CO_ERROR_NO;

    //*************************************
    // set sync
    //*************************************
    ODR_t odRet;
    // Set SYNC Period 100000μsec
    odRet = OD_set_u32(OD_ENTRY_H1006_communicationCyclePeriod, 0, 100000, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_ENTRY_H1006_communicationCyclePeriod);
        return CO_ERROR_OD_PARAMETERS;
    }
    // Enable SYNC
    odRet = OD_set_u32(OD_ENTRY_H1005_COB_ID_SYNCMessage, 0, 0x40000080, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_ENTRY_H1005_COB_ID_SYNCMessage);
        return CO_ERROR_OD_PARAMETERS;
    }

    //*************************************
    // Set RPDO for Right Motor
    // 初期化にて動的にパラメータを設定していますが、CANopenEditorで予め設定すること
    // 生成されたod.cを修正することもOKです。
    //*************************************
    odRet = OD_set_u32(OD_ENTRY_H1400_RPDOCommunicationParameter, 1, 0x0000038A, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_ENTRY_H1400_RPDOCommunicationParameter);
        return CO_ERROR_OD_PARAMETERS;
    }
    odRet = OD_set_u8(OD_ENTRY_H1600_RPDOMappingParameter, 0, 2, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_ENTRY_H1600_RPDOMappingParameter);
        return CO_ERROR_OD_PARAMETERS;
    }
    odRet = OD_set_u32(OD_ENTRY_H1600_RPDOMappingParameter, 1, 0x60410010, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_ENTRY_H1600_RPDOMappingParameter);
        return CO_ERROR_OD_PARAMETERS;
    }
    odRet = OD_set_u32(OD_ENTRY_H1600_RPDOMappingParameter, 2, 0x60640020, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_ENTRY_H1600_RPDOMappingParameter);
        return CO_ERROR_OD_PARAMETERS;
    }
    /* コールバックハンドラの登録 */
    /* 右のハンドラに渡すオブジェクトを生成してnode id(0x038A)を設定する　*/
    uint16_t *objR = (uint16_t*)malloc(sizeof(uint16_t));
    *objR = 0x038A;
    H6064_extentionR.object = objR;
    OD_extension_init(OD_ENTRY_H6064_positionActualValue, &H6064_extentionR);

   //*************************************
    // Set RPDO for Left Motor
    // 初期化にて動的にパラメータを設定していますが、CANopenEditorで予め設定すること
    // 生成されたod_2nd.cを修正することもOKです。
    //*************************************
    odRet = OD_set_u32(OD_2nd_ENTRY_H1401_RPDOCommunicationParameter, 1, 0x0000038B, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_2nd_ENTRY_H1401_RPDOCommunicationParameter);
        return CO_ERROR_OD_PARAMETERS;
    }
    odRet = OD_set_u8(OD_2nd_ENTRY_H1601_RPDOMappingParameter, 0, 2, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_2nd_ENTRY_H1601_RPDOMappingParameter);
        return CO_ERROR_OD_PARAMETERS;
    }
    odRet = OD_set_u32(OD_2nd_ENTRY_H1601_RPDOMappingParameter, 1, 0x60410010, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_2nd_ENTRY_H1601_RPDOMappingParameter);
        return CO_ERROR_OD_PARAMETERS;
    }
    odRet = OD_set_u32(OD_2nd_ENTRY_H1601_RPDOMappingParameter, 2, 0x60640020, true);
    if (odRet != ODR_OK) {
        if (errInfo != NULL) *errInfo = OD_getIndex(OD_2nd_ENTRY_H1601_RPDOMappingParameter);
        return CO_ERROR_OD_PARAMETERS;
    }
    /* コールバックハンドラの登録 */
    /* 左のハンドラに渡すオブジェクトを生成してnode id(0x038B)を設定する　*/
    uint16_t *objL = (uint16_t*)malloc(sizeof(uint16_t));
    *objL = 0x038B;
    H6064_extentionL.object = objL;
    OD_extension_init(OD_2nd_ENTRY_H6064_positionActualValue, &H6064_extentionL);

    return CO_ERROR_NO;
}

canopendプログラムを起動し、cocommを使ってネットワークステータスをOperationalにすると次のようにコンソールにログが出力され、エンコーダ情報が左右区別されて取得できていることが確認できます。

$ ./canopend can0 -i 1 -c "local-/tmp/CO_command_socket" | grep H6064
H6064_write  posL = 449249
H6064_write  posR = 874
H6064_write  posL = 449252
H6064_write  posR = 874
H6064_write  posL = 449251
H6064_write  posR = 876

このときのcandumpは下記の通り。

$ candump -x -ta can0
 (1692233198.895197)  can0  TX - -  080   [0]
 (1692233198.897146)  can0  RX - -  38B   [6]  60 12 E1 DA 06 00
 (1692233198.897148)  can0  RX - -  38A   [6]  60 12 6A 03 00 00
 (1692233198.995500)  can0  TX - -  080   [0]
 (1692233198.997397)  can0  RX - -  38B   [6]  60 12 E4 DA 06 00
 (1692233198.997400)  can0  RX - -  38A   [6]  60 12 6A 03 00 00
 (1692233199.095734)  can0  TX - -  080   [0]
 (1692233199.097644)  can0  RX - -  38B   [6]  60 12 E3 DA 06 00
 (1692233199.097646)  can0  RX - -  38A   [6]  60 12 6C 03 00 00

次回予定

車輪の回転情報からAGVのオドメトリを生成するために必要な左右のモーターの角度情報を取得することができるようになりました。

次回はPDOを使ってモーターを駆動します。

3
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
3
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?