B3Mサーボモータを動かそう(Python編(3))
この記事では、B3MサーボをPythonで制御する方法を紹介します。第3回はREADコマンドについての解説です。
READコマンドは、B3MのRAMに展開したデータを、アドレス指定により読み込むことができるコマンドです。READコマンドを使用することでサーボの現在の角度や温度、電流値を読み込むことができます。また、WRITEコマンドで書き換えたデータの確認にも便利です。
【過去の記事】
第1回 B3Mサーボモータを動かそう(準備編(1))（改訂版）
第5回 B3Mサーボモータを動かそう(Arduino制御編)（改訂版）
第6回 B3Mサーボモータを動かそう(Python編(1))（改訂版）
第7回 B3Mサーボモータを動かそう(Python編(2))
B3Mの通信プロトコルについては『B3Mソフトウェアマニュアル』をご参照ください。
B3Mサーボの紹介、制御に必要な製品については『B3Mサーボモータを動かそう(Python編(1))』
をご参照ください。
【B3Mの設定】
以下のサンプルプログラムでは、B3Mサーボの設定は工場出荷状態のID0、通信速度1500000bps、個数は1個とします。
ID番号を変更することで複数のサーボをデイジーチェーン接続して制御することができますが、通信速度はすべて同じである必要があります。
■READコマンドで読み込むことができるパラメータ
・現在位置
サーボの現在位置を取得します。
・現在速度
サーボの現在の速度を知ることができます。（位置制御モード以外で有効）
・起動時からの経過時間
サーボが起動してから現在までの経過時間を取得できます。
・現在のMCU温度、モーター温度、（負荷）電流値、入力電圧値
サーボ内部のセンサ、回路によりサーボの温度、電流値などを取得できます。
・現在のPWMカウント値
PWMのデューティ値を表示します。
※数値はPWM Frequencyに対するカウント数で、いわゆるデューティ比ではありません。
その他、WRITEコマンドの記事で紹介した各アドレスのパラメータを読み込むことも可能です。
■READコマンドを作成する
|
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 |
def B3M_Read_CMD(servo_id, Data_size, Address): #送信コマンドを作成 txCmd = [0x07, #SIZE 0x03, #CMD 0x00, #OPTION servo_id, #ID Address, #ADDRRESS Data_size] #LENGTH #チェックサムを用意 checksum = 0 for i in txCmd: checksum += i #リストの最後にチェックサムを挿入する txCmd.append(checksum & 0xff) #WRITEコマンドを送信 b3m.write(txCmd) #サーボからの返事を受け取る rxCmd = b3m.read(5+Data_size) #もしリスト何になにも入っていなかったら正常に受信できていないと判断 if len(rxCmd) == 0: return False, 0 #問題なければデータを返す #1Byteの場合 if Data_size == 1: return True, rxCmd[4] #2Byteの場合 elif Data_size == 2: Value = [rxCmd[4], rxCmd[5]] #データの範囲にマイナスが含まれる場合 if (Address == 0x05 #最小位置制御 or Address == 0x07 #最大位置制御 or Address == 0x09 #中央値オフセット or Address == 0x0B #MCU温度リミット or Address == 0x0E #モーター温度リミット or Address == 0x2A #目標位置 or Address == 0x2C #現在位置 or Address == 0x2E #前回のサンプリングの位置 or Address == 0x30 #目標速度 or Address == 0x32 #現在速度 or Address == 0x34 #前回のサンプリングの速度 or Address == 0x3C #目標トルク or Address == 0x44 #現在のMCU温度 or Address == 0x46):#現在のモーター温度 Value = int.from_bytes(Value, 'little', signed=True) #データの範囲がプラスのみの場合 else: Value = int.from_bytes(Value, 'little') return True, Value #4Byteの場合 elif Data_size == 4: Value = [rxCmd[4], rxCmd[5], rxCmd[6], rxCmd[7]] Value = int.from_bytes(Value, 'little') return True, Value else: return False, 0 |
ホストから送信するコマンド体系は、これまで解説したWRITEコマンドやSET POSITIONコマンドと同じです。CMDを0x03に指定することでREADコマンドとして機能します。読みたいデータのアドレスと、データのバイト数を指定してpySerialのwrite関数でサーボに送信します。
rxCmd = b3m.read(5+Data_size)
write関数で送信したコマンドをサーボが問題なく受信すると、サーボから返事が返ってきます。これをpySerialのread関数で受け取ります。read関数の引数で渡す数値は、返事の総バイト数です。読み込むデータのバイト数によりサイズが変化しますので(5+Data_size)でサイズの異なるデータにも対応できるようにします。
受け取った後は、データのバイト数により処理が異なります。サンプルでは、if文により処理を分岐しました。
・1バイトの場合
return True, rxCmd[4]
1番目の戻り値のTrueは、サーボから正常に返事が返ってきたことを呼び出し元に伝えるために書きました。データは、サーボからの返事で受け取ったリストの5番目に格納されていますので、returnでそのまま戻り値にします。
・2バイトの場合
Value = [rxCmd[4], rxCmd[5]]
Value = int.from_bytes(Value, 'little')
返事で受け取ったリストの5番目と6番目にデータが格納されています。このデータはリトルエンディアンで受け取っていますので、元の数値に組み合わせるためpySerialのint.from_bytes関数を使用します。
また、16進数のデータの場合、0～プラスの値はそのまま10進数に変換しても問題ありませんが、マイナスのデータは-1が0xFFFFになっています。例えば、現在角度のデータを取得した場合、時計回りの正転方向のデータ（プラスのデータ）は、0x0001～0x7d00（1～32000）のデータですが、反時計回り（マイナスのデータ）は0xFFFF～0x8300となっています。-1は0xFFFFです。
これをそのまま10進数に変換すると65535（-1）～33536（-32000）となり、サーボの角度がわかりません。そこで、データの範囲がマイナスを含む場合は、pySerialのint.from_bytes関数の引数にsigned=Trueを記述します。これにより、int.from_bytes関数に渡したデータが、自動で-1～の値に変換されます。
また、B3Mは2バイトデータでも符号なしで0～65535で受け取るデータもありますので、signed=Trueを記述するかの判断をアドレスによりif文で分岐しています。
最後にreturn True, Valueで、データを戻します。
・4バイトの場合
Value = [rxCmd[4], rxCmd[5], rxCmd[6], rxCmd[7]]
Value = int.from_bytes(Value, 'little')
return True, Value
2バイトのデータと同じようにリトルエンディアンのデータを元のデータに組み合わせてからreturnでデータを戻します。4バイトの場合は、マイナスになるパラメータはありませんので、2バイトのような処理は必要ありません。
■READコマンドを使ってみる
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 |
#COMポートを開く b3m = serial.Serial('COM4', baudrate=1500000, parity=serial.PARITY_NONE, timeout=0.5) #ID番号を読み込む #B3M_Read_CMD(servo_id, Data_size, Address) bl, reData = B3M_Read_CMD(0, 1, 0x00) print(reData) #現在位置を読み込む bl, reData = B3M_Read_CMD(0, 2, 0x2C) print(reData) #通孫速度を読み込む bl, reData = B3M_Read_CMD(0, 4, 0x01) print(reData) #ポートを閉じる b3m.close() |
実際にREADコマンドでB3Mサーボのデータを読んでみます。B3M_Read_CMDの引数にID番号、データサイズ、アドレスを指定します。サンプルでは、1バイト、2バイト、4バイトデータがそれぞれ正常に機能しているか確認するために、ID（1バイト）、現在位置（2バイト）、通信速度（４バイト）のデータを読みました。
B3Mサーボの設定がID0、通信速度1500000bpsの場合は、コンソールに0、1500000と表示されます。現在位置はサーボの角度により異なるデータが返ってきます。現在はサーボの軸が脱力した状態だと思いますので、手で動かしながら正しくデータを読めているか確認してください。
プログラムの全体は、下記からダウンロードしてください。
サンプルプログラム＜ダウンロード＞
以上でREADコマンドの解説は終了です。READコマンドによりサーボの状態を確認しながら制御できますので、サーボの運用に役に立つと思います。
次回は、これまで作成したコマンドを利用して速度制御への設定変更と、速度を指定し動かす方法をご紹介します。
次の記事『B3Mサーボモータを動かそう(Python編(4))』
前の記事『B3Mサーボモータを動かそう(Python編(2))』
B3M-SB-1040-Aの詳細をみる B3M-SC-1040-Aの詳細をみる B3M-SC-1170-Aの詳細をみる