4
3

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.

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

Last updated at Posted at 2023-08-29

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

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

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

  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ブランチです。

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

状態遷移表

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

DEV_state.png

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

status_machine.png

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

6040.png

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

6041.png

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

velocityのODエントリを追加.png

追加ができたら、「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にする。

TPDO.gif

次回予定

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

4
3
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
4
3

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?