B3Mサーボモータを動かそう(Python編(4))
この記事では、B3MサーボをPythonで制御する方法を紹介します。今回は、制御モードを変更し速度制御でサーボを動かす方法をご紹介します。
速度制御とは、指定した一定の速度で軸が回転する制御です。位置制御では、目標角度を指定し決められた位置で軸が停止しますが、速度制御では目標速度を指定し、軸が回転し続けます。プーリーの回転や、車輪ロボットのタイヤの動力として使用可能です。
以下の解説では、速度制御への設定方法と目標速度の指定方法をご紹介します。
【過去の記事】
第1回 B3Mサーボモータを動かそう(準備編(1))（改訂版）
第5回 B3Mサーボモータを動かそう(Arduino制御編)（改訂版）
第6回 B3Mサーボモータを動かそう(Python編(1))（改訂版）
第7回 B3Mサーボモータを動かそう(Python編(2))
第8回 B3Mサーボモータを動かそう(Python編(3))
B3Mの通信プロトコルについては『B3Mソフトウェアマニュアル』をご参照ください。
B3Mサーボの紹介、制御に必要な製品については『B3Mサーボモータを動かそう(Python編(1))』
をご参照ください。
【B3Mの設定】
以下のサンプルプログラムでは、B3Mサーボの設定は工場出荷状態のID0、通信速度1500000bps、個数は1個とします。
ID番号を変更することで複数のサーボをデイジーチェーン接続して制御することができますが、通信速度はすべて同じである必要があります。
■B3Mサーボが速度制御で動作するまでの流れ
B3Mが電源ONから動き出すまでの制御の流れを説明します。
【動作するまでの流れ】
1.サーボの状態をFreeモードにします。
2.制御モードを「速度制御モード」に変更します。
3.制御モードに応じてPIDゲインのプリセットを変更します。
4.サーボの状態をNomalモードにします。（サーボがトルクONします）
5.目標値の速度を指定します。
B3Mの設定や速度を指定するにはWRITEコマンドを利用します。WRITEコマンドは、B3MサーボのRAMのデータを書き換えるコマンドです。WRITEコマンドの詳細は、『B3Mサーボモータを動かそう(Python編(1))』及び『B3Mサーボモータを動かそう(Python編(2))』をご参照ください。
■プログラム解説
|
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 |
MOVE_NORMAL = 0x00 MOVE_FREE = 0x02 MOVE_HOLD = 0x03 CONTROL_POSITION = 0x00 CONTROL_SPEED = 0x04 CONTROL_CURRENT = 0x08 CONTROL_FEED_FORWARD = 0x0C #COMポートを開く b3m = serial.Serial('COM4', baudrate=1500000, parity=serial.PARITY_NONE, timeout=0.5) #動作モード：Free (動作モードと特性を設定するため、設定書き換え中の誤動作を防止するため脱力にしておく) ModeSelect = CONTROL_POSITION or MOVE_FREE reData = B3M_Write_CMD(0, ModeSelect, 1, 0x28) print(reData) #速度制御モードに設定 (目標速度を指定して軸が回転するモード) ModeSelect = CONTROL_SPEED or MOVE_FREE reData = B3M_Write_CMD(0, ModeSelect, 1, 0x28) print(reData) #ゲインプリセット：No.1 (PIDのプリセットゲインを速度制御モード用に設定) reData = B3M_Write_CMD(0, 0x01, 1, 0x5C) print(reData) #動作モード：Normal （Freeの状態からトルクオン） ModeSelect = CONTROL_SPEED or MOVE_NORMAL reData = B3M_Write_CMD(0, ModeSelect, 1, 0x28) print(reData) #目標速度を指定して回転(100deg/s)(アドレス：0x30（目標速度))(正回転) reData = B3M_Write_CMD(0, 10000, 2, 0x30) print(reData) #0.1秒ごとに現在値を100回取得する for i in range(100): #現在位置を読んでコンソールに表示 reData = B3M_Read_CMD(0, 2, 0x2C) print('現在位置= ', reData) time.sleep(0.1) #指定した秒数の間処理を停止 #目標速度を指定して回転(0deg/s)(アドレス：0x30（目標速度))目標速度を0に指定し、回転を止める reData = B3M_Write_CMD(0, 0, 2, 0x30) print(reData) #目標速度を指定して回転(-100deg/s)(アドレス：0x30（目標速度))（逆回転） reData = B3M_Write_CMD(0, -10000, 2, 0x30) print(reData) #0.1秒ごとに現在値を100回取得する for i in range(100): #現在位置を読んでコンソールに表示 reData = B3M_Read_CMD(0, 2, 0x2C) print('現在位置= ', reData) time.sleep(0.1) #指定した秒数の間処理を停止 #目標速度を指定して回転(0deg/s)(アドレス：0x30（目標速度))目標速度を0に指定し、回転を止める reData = B3M_Write_CMD(0, 0, 2, 0x30) print(reData) #ポートを閉じる b3m.close() |
これまでの記事で解説したCOMを開く方法やWRITEコマンドの詳細は省略します。
MOVE_NORMAL = 0x00
MOVE_FREE = 0x02
MOVE_HOLD = 0x03
CONTROL_POSITION = 0x00
CONTROL_SPEED = 0x04
CONTROL_CURRENT = 0x08
CONTROL_FEED_FORWARD = 0x0C
ModeSelect = CONTROL_POSITION or MOVE_FREE
reData = B3M_Write_CMD(0, ModeSelect, 1, 0x28)
B3M_Write_CMDの引数は(サーボID, データ, データサイズ, アドレス)です。
制御モードを変更するときに誤動作を防ぐため、最初はサーボを脱力した状態に指定します。サーボの制御モード、動作モードの変更は、アドレス0x28を書き換えることで設定できます。
アドレス0x28のモード切替は、制御モードと動作モードの組み合わせでデータを用意します。数値だけではどのモードに変更するのかわかりづらくなるため、変更に必要な部分のbitを立てた変数を用意しておき、送信する値をorで計算するようにプログラムしました。
CONTROL_POSITION(0x00)とMOVE_FREE(0x02)を足し合わせ、0x02を用意し、WRITEコマンドでサーボに書き込むとサーボが脱力します。
（CONTROL_SPEED(0x04)、CONTROL_CURRENT(0x08)、CONTROL_FEED_FORWARD(0x0C)でも、MOVE_FREE(0x02)を指定すれば脱力します）
ModeSelect = CONTROL_SPEED or MOVE_FREE
reData = B3M_Write_CMD(0, ModeSelect, 1, 0x28)
この行で、制御モードを速度制御に指定します。CONTROL_SPEED(0x04)とMOVE_FREE(0x02)を足し合わせ、0x06を用意し、WRITEコマンドでサーボに書き込みます。
reData = B3M_Write_CMD(0, 0x01, 1, 0x5C)
B3Mサーボは位置、速度、電流制御で適切なPIDゲインを変える必要があります。PIDプリセットゲイン(アドレス0x5C)を変えることで、保存されているPIDゲインを一度に変更することができます。
出荷状態では、
PIDプリセットゲイン(アドレス：0x5C)
0x00:位置制御のPIDゲイン
0x01:速度制御のPIDゲイン
0x02:電流制御のPIDゲイン
に保存しています。今回は速度制御ですので0x01を指定しました。
ModeSelect = CONTROL_SPEED or MOVE_NORMAL
reData = B3M_Write_CMD(0, ModeSelect, 1, 0x28)
この行でいよいよサーボのトルクをオンにします。MOVE_FREEからMOVE_NORMAL(0x00）に変更し、WRITEコマンドで書き込みます。
以上までを実行すると、サーボの出力軸に力が入った状態になります。
reData = B3M_Write_CMD(0, 10000, 2, 0x30)
アドレス0x30の目標速度にデータを書き込むとB3Mの軸が回転します。正回転の場合は、1～32767、逆回転の場合は-1～-32767を指定します。このパラメータは、角速度を指定する数値です。1秒間で30度移動する速度を指定する場合は、3000を書き込みます。サンプルの10000の場合は、1秒間に100度移動する速度になります。
reData = B3M_Write_CMD(0, 0, 2, 0x30)
目標速度を0に設定するとサーボの回転が停止します。
reData = B3M_Read_CMD(0, 2, 0x2C)
print('現在位置= ', reData)
前回の記事でREADコマンドについて解説しました。このサンプルプログラムでは、READコマンドを利用し、回転中の現在位置を読み込むようにプログラムしました。他のアドレスを指定すれば、B3Mサーボから現在の温度や電流値を読むことも可能です。
サンプルプログラムを実行し、正回転したあとに逆回転すれば成功です。
プログラムの全体は、下記からダウンロードしてください。
サンプルプログラム＜ダウンロード＞
このサンプルプログラム内には、これまで紹介したすべての関数と、第2回で解説したWRITE関数に合わせた位置制御のプログラムも含まれています。
以上で速度制御で動かす解説を終了します。B3Mサーボには多彩な機能が備わっています。これまで解説したWRITEコマンド、SET POSITIONコマンド、READコマンド、SAVEコマンドを使用すればB3Mサーボを簡単に制御することが可能です。
サンプルプログラム内には、RESETコマンドも用意しましたのでご利用ください。
前の記事『B3Mサーボモータを動かそう(Python編(3))』
B3M-SB-1040-Aの詳細をみる B3M-SC-1040-Aの詳細をみる B3M-SC-1170-Aの詳細をみる