PMXサーボを使ってみよう マイコンボードから制御「MemREAD編」
今回の記事から具体的にPMXサーボを制御する方法を解説します。まずはサーボから情報を読み出す「MemREAD」コマンドについて解説します。
【関連記事】
PMXサーボを使ってみよう マイコンボードから制御「準備編」
この記事で解説するArduino向けライブラリ、及びサンプルプログラムは、下記のページからダウンロードしてください。
PMXシリーズのサーボモータを制御するためのオンラインマニュアルを公開しています。PMXサーボを制御するための詳しい情報が記載されていますので、下記の解説と一緒にご一読ください。
■MemREADコマンドについて
MemREADコマンドは、メモリマップのRAM領域から情報を読み出すためのコマンドです。PMXサーボのメモリマップにはRAM領域とROM領域があります。サーボ起動時にROM領域からRAM領域にデータが展開され、サーボの制御にはRAM領域のデータが使用されます。
このコマンドを使用することで下記の情報を読み出すことができます。
・サーボの現在の状態（現在位置、速度、消費電流、入力電圧など）
・PIDゲインなど制御に関連する設定値
・入力電圧最小値設定など制限値の設定値
・トルクON/OFFの状態、現在の制御モードなど
メモリマップの内容や、各設定値の解説はオンラインマニュアルの『6.メモリマップ』をご参照ください。
なお、サーボIDや通信速度、パリティの読み出しはSystemREADコマンドを使用します。こちらの使用方法については別の記事で解説します。
■コマンドの使用方法
MemREADコマンドは、「先頭アドレス」と「読出データ数」を指定してデータを読み出します。
「先頭アドレス」とは、メモリマップのアドレスを指し、読み出すデータの先頭のアドレスを指定することができます。下図はメモリマップの一部を抜粋した表です。たとえば「現在位置」を読み出す場合、アドレスは300ですので、先頭アドレスとして黄色いセルの300を指定します。
「読出データ数」は、先頭アドレスから何バイトのデータを読み出すかを指定することができます。「現在位置」のみを読み出す場合は、「現在位置」のデータは2バイトですので、「読出データ数」は2となります。また「現在位置」から「現在電流値」までを一気に読み出す場合は、各2バイトのデータが3つ選択しますので6と指定します。つまり、下図の緑のセルの内容が読み出されます。
メモリマップの詳細は、オンラインマニュアルの『メモリマップ一覧』をご参照ください。こちらに各データのアドレスとデータバイト数が一覧になっています。
■リトルエンディアンについて
PMXサーボの2バイト以上のデータは、リトルエンディアンに分割されて格納されています。データを読み出す場合は、読み出したデータをリトルエンディアン方式で変換してご利用ください。詳しくはオンラインマニュアルの『データ構造』にある「データ」の解説をご参照ください。
■プログラム解説
ここからArduinoライブラリのサンプルプログラム「MemREAD_Sample」を使用してMemREADコマンドを実行し、PMXサーボからデータを読み出す方法をご紹介します。
なお、本サンプルはArduino Nano Everyで使用することを想定していますが、ピン番号やシリアルのポート番号を変更することにより多数のArduinoに対応します。使用できるシリアルポートなどについてはArduinoの仕様をご確認ください。解説の「●ライブラリを使用するための準備と通信の設定」がボードに依存する設定項目です。これ以降はすべてのボードに対して共通で使用できます。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
#include <Arduino.h> #include <PmxHardSerialClass.h> #include <DataConvert.h> // サーボとArduino間の通信設定 const byte EN_PIN = 2; // EN(enable)ピンのピン番号 const long BAUDRATE = 115200; // 通信速度[bps] const int TIMEOUT = 1000; // タイムアウトまでの時間[ms] // インスタンス＋ENピン(2番ピン)およびUARTの指定 PmxHardSerial pmx(&Serial1,EN_PIN,BAUDRATE,TIMEOUT); const byte ServoID = 0; //サーボのID番号 |
まずは必要なファイルやライブラリを参照し、PMXと通信を行うクラス(PmxHardSerial)をインスタンス化して使用できるようにします。
「const byte EN_PIN = 2;」は、Enableを指定するピン番号です。こちらはデジタルI/Oで入出力が可能であればどの端子でも構いません。
通信速度は、サーボに設定されている通信速度です。工場出荷状態では115200bpsに設定されています。
タイムアウトは、なんらかのトラブルによりサーボからの返事が返ってこない場合の待ち時間です。単位は[ミリ秒]ですので1000は1秒になります。短くしすぎると正常にパケットを受け取れない場合がありますのでご注意ください。
「pmx(&Serial1,EN_PIN,BAUDRATE,TIMEOUT);」の「&Serial1」がシリアルポートを指定する引数です。サンプルプログラムでは、Serial1をPmxHardSerialクラスの中で直接使用するので「&」をつけてポインタを渡します。「RS-485変換基板」からArduinoに接続した「R」「D」のポート番号を指定してください。
「const byte ServoID = 0;」でこのサンプルプログラムで制御するサーボを指定します。今回はID0のサーボと通信しますので"0"を代入しています。
|
1 2 3 4 5 6 |
void setup() { Serial.begin(115200); // PCと通信を開始する delay(500); // サーボが起動するまで少し待つ pmx.begin(); // サーボモータの通信初期設定 |
「pmx.begin()」は、サーボと通信を開始するための関数です。この関数が記述されている場所からArduinoがサーボと通信を開始します。同時にサーボから読み出したデータをPCに表示するため「Serial.begin(115200);」でArduinoとPC間の通信を開始しています。プログラムの最初に1度だけsetup()が実行されますのでここに記述してください。
●基本のMemREAD()関数
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 |
uint16_t flag; unsigned short address = 0; // MemRead関数を直接使ってステータス情報を呼び出す(返信データはbyte配列) // 300番地はサーボの状態を読み出すことができます。 // 6bytes読み出すことで現在位置、現在速度、現在電流値を一気に読み出せます。ただし、動作していませんので現在速度は0、現在電流値は微量な数値になります。 Serial.println("300番地(現在位置)から6byte読み出します(返り値はbytes型)"); byte rxData[6]; // サーボから読み出したデータを受け取るための配列 flag = pmx.MemREAD(ServoID, PMX::RamAddrList::NowPosition, 6, rxData); // MemREADコマンドを実行 if(flag == 0) // flagが0だった時正常にデータが返ってきている { for(int i = 0; i < 5; i++){ Serial.print(rxData[i]); // 読み出したデータをそのまま表示 Serial.print(", "); } Serial.println(rxData[5]); } else { Serial.print("Error="); Serial.println(flag, HEX); } // pmx.MemREAD()で読み出したデータはリトルエンディアンで分割されています。以下の関数で一つのデータに連結することができます。 // 例として現在位置のみ連結します。 if(flag == 0) // flagが0だった時正常にデータが返ってきている { int posData; byte rxPosByte[2] = {rxData[0], rxData[1]}; // 現在位置のデータのみ抜粋 posData = DataConv::bytesToInt16(rxPosByte); // 2Byteのデータを連結 Serial.print("Position Data="); Serial.println(posData); } |
この行からMemREADコマンドを実行します。PMXサーボのPythonライブラリには、MemREADコマンドを実行するために多彩な関数を用意しました。上記の関数はMemREADコマンドの基本となる関数です。
第一引数はサーボIDです。「ServoID」は、MemREADコマンドの命令の対象になるサーボを指定しています。前のプログラムで「ServoID = 0」と代入していますので、この関数はID0のサーボに対して命令を送信しています。
第二引数は、読み出すデータが格納されているメモリの先頭のアドレスを指定します。「PMX::RamAddrList::NowPosition」は、「現在位置」のアドレスを示しライブラリで300が代入されています。またアドレスの数値として「300」と指定しても同じ番地からデータを読み出すことができます。
第三引数は読出データ数です。6が指定されていますので、先頭アドレス300から6バイトのデータを読み出します。
第四引数の「rxData」にMemREADコマンドで読み出したデータが代入されます。Arduino言語のベースとなるC++は、複数の戻り値を返すことができませんので、予め作成しておいた変数、配列のポインタを引数で渡し、関数内でここに代入してもらうことでデータを受け取ります。戻り値で受け取るデータは、サーボの状態や通信状態を知ることができるステータスのみ返ってきます。
戻り値の「flag」は、サーボとの通信状態とサーボの状態が正常かどうかのステータスを足したものが返ってきます。データが0であれば問題ありません。0以外の場合は、なんらかの問題が発生していますので、ステータス部がエラーの場合はオンラインマニュアルの『5.エラー状態』を参照してください。
前述の通り、2バイト以上のデータはリトルエンディアンで分割されていますので連結する必要があります。本ライブラリには、リトルエンディアンで分割されたデータを組み合わせるために便利な関数が用意されています。
|
1 2 3 4 5 6 7 8 9 10 |
// pmx.MemREAD()で読み出したデータはリトルエンディアンで分割されています。以下の関数で一つのデータに連結することができます。 // 例として現在位置のみ連結します。 if(flag == 0) // flagが0だった時正常にデータが返ってきている { int posData; byte rxPosByte[2] = {rxData[0], rxData[1]}; // 現在位置のデータのみ抜粋 posData = DataConv::bytesToInt16(rxPosByte); // 2Byteのデータを連結 Serial.print("Position Data="); Serial.println(posData); } |
2Byte以上の分割されたのデータを都度数値に変換するとプログラムが長くなりますので、DataConvクラスを用意しました。DataConv::bytesToInt16()でデータを連結することができます。この関数に連結する配列を渡します。
今回は例として6バイトのデータを読み出しましたが「現在位置」は先頭の2バイトのみですので、「byte rxPosByte[2] = {rxData[0], rxData[1]};」で「現在位置」のデータのみ別の配列に分けます。
※pmx.MemREAD()で2バイトのみ読み出した場合は、そのまま渡しても問題ありません。
「DataConv::bytesToInt16(rxPosByte);」を実行すると、戻り値として連結したデータが返ってきます。このデータを「Serial.println(posData);」によりArduino IDEのシリアルモニタに表示することができます。
成功すると、現在の値が表示されます。角度値は角度x100のデータですので、100度は10000のデータで返ってきます。実際のサーボの角度と照らし合わせてご確認ください。
PMXのデータは、符号の有無、Byte数で複数の種類が存在します。今回使用した「bytesToInt16()」は符号あり2Byteのデータを対象としていますが、他のデータ形式に対応した関数もご用意しています。下記を参考に使い分けてください。
|
1 2 3 4 5 6 7 8 9 10 11 |
// 分割された2Byte(符号あり)データをまとめる static short bytesToInt16(unsigned char byteDatas[]); // 分割された2Byte(符号なし)データをまとめる static unsigned short bytesToUint16(unsigned char byteDatas[]); // 分割された4Byte(符号あり)データをまとめる static long bytesToInt32(unsigned char byteDatas[]); // 分割された4Byte(符号なし)データをまとめる static unsigned long bytesToUint32(unsigned char byteDatas[]); |
●1つにまとまったデータを受け取ることができるMemREADコマンドの便利な関数
基本的な使い方は以上ですが、MemREAD()で読み出したデータを毎度bytesToInt16()などで連結するとプログラムの行数が増えてしまいます。そこで、戻り値として連結したデータを受け取ることができる関数を用意しました。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 |
// MemREAD関数を使ってを現在位置（単位[1/100度]）(2byte符号あり)を取得する Serial.println("2byte(signed)の位置データを取得します"); short int16Data; // サーボから読み出したデータを受け取るための変数 flag = pmx.MemREADToInt16(ServoID, PMX::RamAddrList::NowPosition, &int16Data); if(flag == 0) { Serial.print("int16Data="); Serial.println(int16Data); } else { Serial.print("Error="); Serial.println(flag, HEX); } // MemRead関数を使って電圧(単位[mV])(2byte符号なし)を取得する Serial.println("2byte(unsigned)の電圧データを取得します"); unsigned short uint16Data; // サーボから読み出したデータを受け取るための変数 flag = pmx.MemREADToUint16(ServoID, PMX::RamAddrList::InputVoltage, &uint16Data); if(flag == 0) { Serial.print("uint16Data="); Serial.println(uint16Data); } else { Serial.print("Error="); Serial.println(flag, HEX); } // MemRead関数を使って位置制御のPゲインを取得する Serial.println("4byte(unsigned)の位置制御のPゲインを取得します"); unsigned long uint32Data; // サーボから読み出したデータを受け取るための変数 flag = pmx.MemREADToUint32(ServoID, PMX::RamAddrList::PositionKp, &uint32Data); if(flag == 0) { Serial.print("uint32Data="); Serial.println(uint32Data); } else { Serial.print("Error="); Serial.println(flag, HEX); } // MemRead関数を使ってトルクスイッチの状態を取得する Serial.println("1byte(unsigned)のトルクスイッチの状態を取得します"); // TorqueSwitch Data: 0x01=TorqueON, 0x02=Free, 0x04=Brake, 0x08=Hold byte byteData; // サーボから読み出したデータを受け取るための変数 flag = pmx.MemREADToByte(ServoID, PMX::RamAddrList::TorqueSwitch, &byteData); if(flag == 0) { Serial.print("byteData="); Serial.println(byteData); } else { Serial.print("Error="); Serial.println(flag, HEX); } |
関数は下記の5種類です。
・MemREADToByte(id, address, rxData) ＝Byte型（1バイト）符号なし
・MemREADToInt16(id, address, rxData) ＝Int型（2バイト）符号あり
・MemREADToUint16(id, address, rxData) ＝Int型（2バイト）符号なし
・MemREADToInt32(id, address, rxData) ＝Int型（4バイト）符号あり
・MemREADToUint32(id, address, rxData) ＝Int型（4バイト）符号なし
いずれも引数にサーボIDとアドレス、基本のMemREAD()関数と同じく読み出したデータを受け取る変数のポインタを渡します。オンラインマニュアルの『メモリマップ一覧』を参照し、型と符号が一致する関数を使用してください。
すべての関数で戻り値はサーボの状態や通信状態が返ってきます。
●各パラメータの専用関数でプログラムを読みやすく
また、読み出すデータによっては、アドレスの指定や読出データ数の指定を省くために専用の関数が用意されています。こちらを使用するとサーボIDと読み出したデータを受け取る変数を渡すことで機能を実行できますので便利です。関数の種類はライブラリをご参照ください。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 |
// ライブラリには専用の関数を用意している場合があります。 // 下記はトルクスイッチの状態を読み出すための関数です。これにより、アドレスを指定する必要がなくなり、プログラムもすっきりします。 // トルクスイッチの状態を取得する Serial.println("トルクスイッチの状態を専用の関数で読み出します"); flag = pmx.getTorqueSwitch(ServoID, &byteData); if(flag == 0) { Serial.print("byteData="); Serial.println(byteData); } else { Serial.print("Error="); Serial.println(flag, HEX); } |
■エラーの解除方法
最後に、サーボにエラーが発生した場合のフラグの解除方法を解説します。受信パケットのステータスにエラーが発生した場合は、受信パケットのステータスがエラー(0以外)になります。同時にアドレス400番台の該当のデータが変化しています。エラーの種類のよっては、安全のためエラーを解除しない限り動作が停止する場合がありますので解除が必要です。
エラーは、アドレス400番台をMemREADコマンドで読み出しますことでエラーを解除することができます。また、getFullStatus()関数を使用するとステータスの情報を一気に読む出すこともできます。
getFullStatus()関数で読み出す場合は、予め「システムエラー」「モータエラー」「RAMアクセスエラー」の情報を格納する変数を用意し、関数の引数にポインタで渡します。
関数実行後に、各変数にデータが入っていますので、printで出力し状態を確認してください。すべて0であれば異常はありません。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
// エラー情報を読み出し、エラーを解除します // エラー情報を読み出すと解除されます // getFullStatus関数を使用することでエラー情報を項目に分けて読み出すことができます。 // 2ByteデータのRAMアクセスエラーはまとまった状態で受け取ることができます。 Serial.println("エラーステータス(400番地から6byte)からエラー情報を読み出します"); byte sysSt; // システムエラーを受け取る変数 byte motorSt; // モータエラーを受け取る変数 unsigned short ramSt; // RAMアクセスエラーを受け取る変数 flag = pmx.getFullStatus(ServoID, &sysSt, &motorSt, &ramSt); // エラー状態を読み出す Serial.print("ステータスエラー"); // 各データを表示 Serial.println(flag); Serial.print("システムエラー="); Serial.println(sysSt); Serial.print("モータエラー="); Serial.println(motorSt); Serial.print("RAMアクセスエラー="); Serial.println(ramSt); |
ただし、電源電圧が低い、高いなど原因が解消していない状態でエラー情報を読み出してもエラーを解除することはできません。エラーが発生した場合は、エラーの内容をよく確認し安全にご利用ください。
エラー情報については、オンラインマニュアルの『5.エラー状態』をご参照ください。
以上でMemREADコマンドの解説を終わります。次回はメモリマップの情報を書き換えることができるMemWRITEコマンドについて解説します。
次の記事『PMXサーボを使ってみよう マイコンボードから制御「MemWRITE編」』
前の記事『PMXサーボを使ってみよう マイコンボードから制御「準備編」』
PMX-SCR-9203HVの詳細をみる PMX-SCR-9204HVの詳細をみる PMX-SCR-5203HVの詳細をみる PMX-SCR-5204HVの詳細をみる