前回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ロボットドライバを作成します。