PMXサーボを使ってみよう マイコンボードから制御「MotorWRITE （位置制御）編」
前回までの解説で、サーボからデータを読み出す「MemREAD」コマンド、データを書き込む「MemWRITE」コマンドの解説が終わりました。今回から、サーボの動作を制御する「MotorWRITE」コマンドを解説し、実際にPMXサーボを動かしたいと思います。
【関連記事】
PMXサーボを使ってみよう マイコンボードから制御「準備編」
PMXサーボを使ってみよう マイコンボードから制御「MemREAD編」
PMXサーボを使ってみよう マイコンボードから制御「MemWRITE編」
この記事で解説するArduino向けライブラリ、及びサンプルプログラムは、下記のページからダウンロードしてください。
PMXシリーズのサーボモータを制御するためのオンラインマニュアルを公開しています。PMXサーボを制御するための詳しい情報が記載されていますので、下記の解説と一緒にご一読ください。
■MotorWRITEコマンドについて
MotorWRITEコマンドは、PMXサーボの動作を指示するためのコマンドです。このコマンドを使用することで、命令を受け取ったサーボは設定された「制御モード」に合わせて動作します。また、パケットを受け取った返事として「応答データ指定」で指定したデータを含んで返事を返します。
■サーボの電源投入から動作までの手順
PMXサーボは、多彩な機能を実行するためにサーボの電源投入から動作開始まで各モードの設定が必要です。
①「制御モード」を指定する
②「応答データ指定」を指定する
③「トルクスイッチ」の設定をTorqueONに指定する
④「MotorWRITE」コマンドで動作指示をする
■プログラム解説
ここからArduinoライブラリのサンプルプログラム「MotorWRITE_Single_Sample」を使用してMotorWRITEコマンドを実行し、PMXサーボを制御する方法を解説します。
MotorWRITEコマンドには多彩な機能が備わっていますが、実例に沿って解説した方がわかりやすいため、プログラムの解説と同時にコマンドについて解説します。
ファイルの参照やライブラリのインスタンス化は『PMXサーボを使ってみよう マイコンボードから制御「MemREAD編」』をご参照ください。
「サーボの電源投入から動作までの手順」①～④の流れは下記になります。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 |
// ①制御モードの設定します // MemWRITEコマンドで第三引数のwriteOptが0x01のときはトルクONでも強制的に書き込めます byte writeOpt = 1; // トルクオンでMemWRITEを実行を許可するか 0：許可しない 1：許可する byte controlMode = PMX::ControlMode::Position; // 位置制御モード値 flag = pmx.setControlMode(ServoID, controlMode, writeOpt); // ②応答データを指定します byte receiveMode = PMX::ReceiveDataOption::Full; // すべて返す flag = pmx.setMotorReceive(ServoID, receiveMode, writeOpt); // ③サーボのトルクをオンにします long receiveData[8]; flag = pmx.setMotorTorqueOn(ServoID, receiveMode, receiveData, controlMode); // ④MotorWRITE関数で動作指令 long pos = -32000; // 目標位置 int writeDataCount = 1; long writeDatas[1] = {pos}; flag = pmx.MotorWRITE(ServoID, writeDatas, writeDataCount, receiveMode, receiveData, controlMode); // 目標地点に到達するまで待ちます delay(2000); |
|
1 2 3 4 5 6 7 |
// 制御モードの設定します // MemWRITEコマンドで第三引数のwriteOptが0x01のときはトルクONでも強制的に書き込めます byte writeOpt = 1; // トルクオンでMemWRITEを実行を許可するか 0：許可しない 1：許可する byte controlMode = PMX::ControlMode::Position; // 位置制御モード値 flag = pmx.setControlMode(ServoID, controlMode, writeOpt); Serial.print("setControlMode="); Serial.println(flag, HEX); |
まず、制御モードを指定します。制御モードは、PMXサーボがどの制御で動作するかを指定することができます。PMXサーボは工場出荷状態で位置制御が選択されていますので、位置制御のみで使用する場合は処理を飛ばしても問題ありません。
Bit0：位置制御
Bit1：速度制御
Bit2：電流制御
Bit3：トルク制御
Bit4：PWM制御
Bit5：移動時間指示
各ビットを1にすることでモードを選択することができます。例えば、位置制御のみの場合は0x01(0b00000001)、位置制御と電流制御を組み合わせる場合は、0x05(0b00000101)となります。こちらのデータをMemWRITEコマンドを使用し、アドレス501の「制御モード」に書き込みます。
ライブラリでは、setControlMode()という制御モードを変更するための専用の関数を用意しています。
第一引数の「ServoID」は、書き込みの対象となるサーボのID番号です。
第二引数の「controlMode」は「制御モード」に書き込むデータです。「制御モード」のデータは、後のMotorWRITEコマンドやMotorREADコマンドでも使用しますので、「controlMode」変数に代入してから引数にしています。直接数値を記入しても同様の処理を実行できます。
第三引数の「writeOpt」は、PMXサーボのトルクスイッチがTorqueONの状態でもMemWRITEコマンドの実行を許可するかを指定します。0は禁止、1は許可になります。動作環境に合わせて選択してください。
制御モードの組み合わせは制限があります。「制御モード」について詳しくはオンラインマニュアルの『制御モード』をご参照ください。
|
1 2 3 4 5 |
// 応答データを指定します byte receiveMode = PMX::ReceiveDataOption::Full; // すべて返す flag = pmx.setMotorReceive(ServoID, receiveMode, writeOpt); Serial.print("setMotorReceive="); Serial.println(flag, HEX); |
次に、応答データを指定します。PMXサーボは、MotorWRITEコマンドやMotorREADコマンドを受け取った返事のパケットに、サーボの状態を示すデータを含めて返すことができます。これを応答データと呼びます。
応答データは下記の8種類です。
Bit0：現在位置
Bit1：現在速度
Bit2：現在電流値
Bit3：現在推定トルク
Bit4：PWM出力割合
Bit5：モータ温度
Bit6：CPU温度
Bit7：入力電圧
各ビットを1にすることで指定することができます。0xFFで8種類すべての応答データを受け取ることもできますし、0を指定すれば応答データを含めずに最小のバイト数で返事のパケットを受け取ることができます。制御に必要なデータを選択してください。
ライブラリのsetMotorReceive()関数を使用することで「応答データ指定」を書き換えることができます。
第一引数の「ServoID」は、書き込みの対象となるサーボのID番号です。
第二引数の「receiveMode」は「応答データ指定」に書き込むデータです。「応答データ指定」の指定データは、後のMotorWRITEコマンドやMotorREADコマンドでも使用しますので、「receiveMode」変数に代入してから引数にしています。直接数値を記入しても同様の処理を実行できます。
第三引数の「writeOpt」は、PMXサーボのトルクスイッチがTorqueONの状態でもMemWRITEコマンドの実行を許可するかを指定します。0は禁止、1は許可になります。動作環境に合わせて選択してください。
「応答データ指定」について詳しくはオンラインマニュアルの『応答データ指定』をご参照ください。
【設定を保存する】
以上①②の設定は、SAVEコマンドを実行することでサーボに保存することができます。保存が完了しましたら、以降は電源投入後③のトルクをオンにする手順からスタートすることでき、トルクオンから即動作することが可能です。
|
1 2 3 4 5 |
// サーボのトルクをオンにします long receiveData[8]; flag = pmx.setMotorTorqueOn(ServoID, receiveMode, receiveData, controlMode); Serial.print("setMotorTorqueOn="); Serial.println(flag, HEX); |
「トルクスイッチ」にデータを書き込んで、サーボをTorqueONモードにします。「トルクスイッチ」には下記の4種類のモードがあります。
・TorqueON setMotorTorqueOn()関数
・Free setMotorFree()関数
・Brake setMotorBrake()関数
・Hold setMotorHold()関数
サーボに電源が入った直後の脱力した状態が「Free」です。サーボが動作するためには必ず「TorqueON」に設定します。「Free」「Brake」「Hold」でMotorWRITEコマンドを受け取ってもサーボは動作しませんのでご注意ください。各モードの詳細は、オンラインマニュアルの『トルクスイッチ』をご参照ください。
ライブラリのsetMotorTorqueOn()関数を使用することでサーボを「TorqueON」の状態にできます。また、それぞれのモードに設定するための関数も用意されています。
なお、setMotorTorqueOn()関数の内部ではMotorWRITEコマンドを実行していますので、戻り値として②「応答データ指定」で指定したサーボの情報を受け取ることができます。
第一引数の「ServoID」は、書き込みの対象となるサーボのID番号です。
第二引数の「receiveMode」は、応答データの内容を関数に伝えるために渡します。トルクスイッチの前に処理した②「応答データ指定」と同じ内容のまま引数として渡してください。setMotorTorqueOn()関数の内部ではMotorWRITEコマンドを実行しているため、返事のパケットに「応答データ指定」で指定したデータが含まれます。
②「応答データ指定」で解説した通り、MotorWRITEの返事としてサーボの情報を受け取ることができます。予め配列（receiveData[8]）を用意して置き、第三引数で渡します。サーボに命令を送り、正常に処理が実行されるとreceiveData[8]に「応答データ指定」で指定したサーボの情報が代入されます。
第四引数は「制御モード」です。こちらは①「制御モード」と同じ内容のまま引数として渡してください。
以上、setMotorTorqueOn()が正常に実行されるとサーボがトルクオンの状態になります。
ここまでの処理でPMXサーボを動作させる準備が整いました。次の処理からMotorWRITEコマンドを使用してサーボを動かします。
●基本のMotorWRITE()関数
|
1 2 3 4 5 6 7 8 9 10 |
// MotorWRITE関数で動作指令 long pos = -32000; // 目標位置 int writeDataCount = 1; long writeDatas[1] = {pos}; flag = pmx.MotorWRITE(ServoID, writeDatas, writeDataCount, receiveMode, receiveData, controlMode); Serial.print("MotorWRITE="); Serial.println(flag, HEX); // 目標地点に到達するまで待ちます delay(2000); |
MotorWRITEコマンドを使用するための基本的な関数はMotorWRITE()です。
第一引数は、対象となるサーボのID番号です。
第二引数は、「動作指示データ」です。各制御モードに合わせた数値を送り、サーボを動かします。位置制御の場合は角度を送ります。例えば、PMX-SCR-5204HVは-320°～320°の範囲で角度を指示できますが、単位は1/100[°]ですので、サーボに送信する「動作指示データ」は-32000～32000の範囲になります。サーボに100°を指示する場合は10000を送信します。
PMXサーボの特徴として、制御を組み合わせることができます。このためMotorWRITE()関数の第五引数の「動作指示データ」はリストになっています。組み合わせが一つの場合はデータも一つ、組み合わせが二つの場合は二つのデータを送ります。今回は組み合わせが一つですので[pos]として一つのデータを引数にしています。二つの場合は[0, 0]のように二つ用意します。
第三引数は「動作指示データ」の数を渡します。今回は1つなので1にしました。上記の通り、制御を組み合わせる場合は、組み合わせた数を第三引数に渡してください。
第四引数～第六引数は「応答データ指定」、応答データを受け取るための配列、「制御モード」です。「制御モード」の内容と、「応答データ指定」で返ってくるデータの内容を関数に伝えるために引数として渡します。上記のプログラムで用意した「controlMode」「receiveMode」変数の内容を変えずにそのまま使用します。
以上の引数を持ったMotorWRITE()関数を、サンプルの通り-32000を指定して実行すると、サーボが-320°の位置に移動します。
サーボがMotorWRITE()を受け取った返事として返信パケットが返ってきます。このパケットの中に「応答データ指定」で指定したデータが含まれています。サンプルでは、次の行のprint()の「reData」で表示されますので内容を確認してください。
●制御の組み合わせで使い分けることができるMotorWRITE()関数
以上でMotorWRITEコマンドの基本的な使い方を解説しました。この方法でサーボを動作させることが可能ですが、より簡単にMotorWRITEコマンドを使用するために以下の関数をご用意しました。
|
1 2 3 4 5 6 7 8 |
// MotorWRITESingle（組合せ1個）で動作指令 long targetVal = -18000; // 目標位置 flag = pmx.MotorWRITESingle(ServoID, targetVal, receiveMode, receiveData, controlMode); Serial.print("MotorWRITE-Single="); Serial.println(flag, HEX); // 目標地点に到達するまで待ちます delay(1000); |
MotorWRITESingle()関数は、制御モードが一つの時に特化した関数です。MotorWRITE()では「動作指示データ」をリストで渡していましたが、この関数では数値のまま渡すことができます。
このほかに、二つの制御モードの時に使用するMotorWRITEDouble()と、制御モードが三つの時のMotorWRITETriple()があります。制御モードの数に合わせて使い分けてください。
●サーボの状態を読み出せるMotorREAD()関数
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 |
// MotorREADで現在の状態を確認 byte torqueSw; flag = pmx.MotorREAD(ServoID, receiveMode, receiveData, controlMode, &torqueSw); Serial.print("MotorREAD"); Serial.println(flag, HEX); Serial.print("TorqueSwitch="); Serial.println(torqueSw); Serial.print("NowPosition="); Serial.println(receiveData[0]); Serial.print("NowSpeed="); Serial.println(receiveData[1]); Serial.print("NowCurrent="); Serial.println(receiveData[2]); Serial.print("NowTorque="); Serial.println(receiveData[3]); Serial.print("NowPWM="); Serial.println(receiveData[4]); Serial.print("NowMotorTemp="); Serial.println(receiveData[5]); Serial.print("NowCpuTemp="); Serial.println(receiveData[6]); Serial.print("NowVoltage="); Serial.println(receiveData[7]); |
最後に、MotorREADコマンドについて解説します。MotorREADコマンドは、「応答データ指定」で指定したデータを読み出すことができます。MotorWRITEコマンドでは、サーボの動作指示が処理に含まれますが、MotorREADコマンドはデータの読み出し処理のみ実行されます。サーボの状態を知りたい場合に便利なコマンドです。
第四引数の「controlMode」は、現在位置のデータ処理に使用します。PMXサーボは、現在位置のデータの仕様が位置制御とそれ以外で異なります。位置制御を含めた制御モードの場合は-32000～32000の範囲になりますが、位置制御を含めない（速度制御、電流制御のみなど）場合は0～35999の範囲になります。これは角度指令で動作する場合と、無限回転で動作する場合で必要な位置情報が異なるため、直感的にわかりやすい数値が返ってくる仕様になっています。
■プログラム全体
|
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 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 |
#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番号 // 制御モードを指定する // 以下の各ビットを1にすると制御モードとして指定できます // Bit7:0 Bit6:0 Bit5:移動時間 Bit4:PWM Bit3:トルク Bit2:電流 Bit1:速度 Bit0:位置 byte controlMode = PMX::ControlMode::Position; // 位置制御モード値 //byte controlMode = 0x01 // 数値でも指定できます。 // 応答データを指定する // 以下の各ビットを1にすると応答データとして指定できます // Bit7:入力電圧 Bit6:CPU温度 Bit5:モータ温度 Bit4:PWM出力 Bit3:推定トルク Bit2:電流 Bit1:速度 Bit0:位置 // receiveMode = PMX.ReceiveDataOption.Position # 位置情報を返す byte receiveMode = PMX::ReceiveDataOption::Full; // すべて返す //byte receiveMode = 0xFF // 数値でも指定できます。 void setup() { Serial.begin(115200); // PCと通信を開始する delay(500); // サーボが起動するまで少し待つ pmx.begin(); // サーボモータの通信初期設定 uint16_t flag; // 制御モードの設定します // MemWRITEコマンドで第三引数のwriteOptが0x01のときはトルクONでも強制的に書き込めます byte writeOpt = 1; flag = pmx.setControlMode(ServoID, controlMode, writeOpt); Serial.print("setControlMode="); Serial.println(flag, HEX); // 応答データ指定します flag = pmx.setMotorReceive(ServoID, receiveMode, writeOpt); Serial.print("setMotorReceive="); Serial.println(flag, HEX); // サーボのトルクをオンにします long receiveData[8]; flag = pmx.setMotorTorqueOn(ServoID, receiveMode, receiveData, controlMode); Serial.print("setMotorTorqueOn="); Serial.println(flag, HEX); } void loop() { uint16_t flag; long receiveData[8]; // MotorWRITE関数で動作指令 long pos = -32000; // 目標位置 int writeDataCount = 1; long writeDatas[1] = {pos}; flag = pmx.MotorWRITE(ServoID, writeDatas, writeDataCount, receiveMode, receiveData, controlMode); Serial.print("MotorWRITE="); Serial.println(flag, HEX); // 目標地点に到達するまで待ちます delay(2000); // MotorWRITESingle（組合せ1個）で動作指令 long targetVal = -18000; // 目標位置 flag = pmx.MotorWRITESingle(ServoID, targetVal, receiveMode, receiveData, controlMode); Serial.print("MotorWRITE-Single="); Serial.println(flag, HEX); // 目標地点に到達するまで待ちます delay(1000); // MotorREADで現在の状態を確認 byte torqueSw; flag = pmx.MotorREAD(ServoID, receiveMode, receiveData, controlMode, &torqueSw); Serial.print("MotorREAD"); Serial.println(flag, HEX); Serial.print("TorqueSwitch="); Serial.println(torqueSw); Serial.print("NowPosition="); Serial.println(receiveData[0]); Serial.print("NowSpeed="); Serial.println(receiveData[1]); Serial.print("NowCurrent="); Serial.println(receiveData[2]); Serial.print("NowTorque="); Serial.println(receiveData[3]); Serial.print("NowPWM="); Serial.println(receiveData[4]); Serial.print("NowMotorTemp="); Serial.println(receiveData[5]); Serial.print("NowCpuTemp="); Serial.println(receiveData[6]); Serial.print("NowVoltage="); Serial.println(receiveData[7]); } |
以上でMotorWRITEコマンドの解説を終わります。今回の記事は、位置制御を中心とした解説となっていますが、PMXサーボは速度制御や電流制御など多彩な制御方法を搭載しています。次回の記事では、位置制御以外の制御についても解説します。
次の記事『PMXサーボを使ってみよう PCから制御「MotorWRITE （組み合わせ制御）編」』
前の記事『PMXサーボを使ってみよう PCから制御「MemWRITE編」』
PMX-SCR-9203HVの詳細をみる PMX-SCR-9204HVの詳細をみる PMX-SCR-5203HVの詳細をみる PMX-SCR-5204HVの詳細をみる