B3Mサーボモータを動かそう(C#位置制御編)
【関連記事】
第1回 B3Mサーボモータを動かそう(準備編(1))（改訂版）
第5回 B3Mサーボモータを動かそう(Arduino制御編)（改訂版）
第6回 B3Mサーボモータを動かそう(Python編(1))（改訂版）
B3Mサーボモータをプログラムで動かす
前回、Visual StudioのC#を用いてライブラリの読み込みや通信設定を行いました。
今回は、実際に角度を指定して、その角度まで動作させてみます。
B3Mサーボモータを動かす手順
B3Mサーボモータを動かす手順は、下記のようになります。
|
①B3Mサーボモータの動作モードをFreeにします |
|
②制御モードを変更します |
|
③制御モードに応じてゲインを変更します |
|
④B3Mサーボモータの動作モードをNormalにします |
|
⑤目標値を設定します |
今回は位置制御モードで角度指令を行いその角度に動くようにします。
出荷設定では位置制御モード、位置制御用のゲインに設定していますので、②、③は省略します。
プログラム作成する前に
今回解説するプログラムではボタン等を用います。
以下がサンプルプログラムのスクリーンショットになります。
それぞれのボタンには下記機能が実装されます。
| Free | B3Mサーボモータの動作モードをFreeにします。 |
| Normal | B3Mサーボモータの動作モードをNormalにします。 |
| SetPos | 角度を指定してB3Mサーボモータを動かします。 |
| getPos | B3Mサーボモータの現在角度を取得します。 |
Normalモードに設定する方法
B3Mサーボモータは電源投入時、意図せず動かないようにFreeに設定されています。
動かすにはFreeからNormal(トルクON)状態にする必要があります。
Normalモードに設定するには、Writeコマンドで0x28番地のデータを変更します。
まずは、0x28番地の値を変更する関数を作成しました。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 |
/// トルクオン、位置制御モード等々一度に設定する private bool b3mModeSet(SerialPort serialPort, byte servoID, byte mode) { ByteList cmd = new ByteList(); //コマンド格納用 byte[] rx = new byte[5]; //コマンド受信用(Writeコマンドは5byte) //コマンドの作成 //cmd.Bytes = B3MLib.B3MLib.Write(0, B3MLib.B3MLib.SERVO_TORQUE_ON, 1, servoID, new byte[] { mode }); //↑V1.0.0.0のB3MLib用 cmd.Bytes = B3MLib.B3MLib.WriteSingle(0, B3MLib.B3MLib.SERVO_TORQUE_ON, servoID, new byte[] { mode }); //option:0 //address:B3MLib.B3MLib.SERVO_TORQUE_ON (0x28) //ID:servoID //data:送信するデータ配列 // コマンドを送信 return B3MLib.B3MLib.Synchronize(serialPort, cmd.Bytes,ref rx); } |
コマンドを送信し、成功するとSynchronizeはtrueを返し、何もデータが返ってこなく、失敗した場合はfalseが返ってきます。
0x28番地は制御モードも含まれますので、Normalモードと位置制御モードの論理和(OR)をとった値になります。
Normalモードにするには、上記関数のmodeにデータを渡すことでモードが変更できます。
|
1 2 3 4 5 6 |
/// /// Normal、位置制御モードにする private bool b3mNomalPosModeSet(SerialPort serialPort, byte servoID) { return b3mModeSet(serialPort, servoID, ((byte)B3MLib.B3MLib.Options.ControlPosition | (byte)B3MLib.B3MLib.Options.RunNormal)); } |
フリーモードも同様に関数化しておきます。
|
1 2 3 4 5 |
/// Free、位置制御モードにする private bool b3mFreePosModeSet(SerialPort serialPort, byte servoID) { return b3mModeSet(serialPort, servoID, ((byte)B3MLib.B3MLib.Options.ControlPosition | (byte)B3MLib.B3MLib.Options.RunFree)); } |
関数化しましたので、あとは実際にこの関数を呼びます。
今回のプログラムでは、ボタンをクリックしたときにこれらの関数を呼ぶようにしました。
ノーマルボタンが押されたときに呼ばれるイベント関数
|
1 2 3 4 5 6 7 8 9 10 11 12 |
/// ノーマルボタンがクリックされたときに呼ばれるイベント private void normalButton_Click(object sender, EventArgs e) { bool flag; flag = b3mNomalPosModeSet(serialPort1, (byte)idNumericUpDown.Value); if (flag == false) { MessageBox.Show("データの送信に失敗しました"); } } |
フリーボタンが押されたときに呼ばれるイベント関数
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
/// フリーボタンがクリックされたとき呼ばれる関数イベント private void freeButton_Click(object sender, EventArgs e) { bool flag; flag = b3mFreePosModeSet(serialPort1,(byte)idNumericUpDown.Value); if (flag == false) { MessageBox.Show("データの送信に失敗しました"); } } |
今回、IDを指定できるよう、NumericUpDownを配置してその値を呼ぶようにしています。
これを実行してみます。
IDをセットして、Freeボタンをクリックすると、脱力するのでホーンが手で回せるようになります。また、Normalボタンをクリックすると手では回らないようになります。
また、IDが違うとエラーメッセージが出ることが確認できると思います。
目標位置を指定する
次に、目標位置をB3Mサーボモータに送ります。
目標位置を設定するには2種類ありますが、今回はPositionコマンドを用います。
これを関数化すると下記の通りになります。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
/// /// 角度指定をする関数 private bool b3mSetPosition(SerialPort serialPort, byte servoID, int angle, ushort time) { ByteList cmd = new ByteList(); //コマンド格納用 byte[] rx = new byte[7]; //コマンド受信用(Writeコマンドは7byte) //コマンドの作成 cmd.Bytes = B3MLib.B3MLib.SetPosision(0, servoID, angle, time); //option:0 //ID:servoID //pos:送信するデータ配列 //time:軌道生成時の時間 // コマンドを送信 return B3MLib.B3MLib.Synchronize(serialPort, cmd.Bytes, ref rx); } |
軌道生成の時間の概念についてはソフトウェアマニュアルをご覧ください。
角度は、実際の角度を100倍した値を用います(1度 => 100になります)。
ボタンをクリックした時に、この関数を呼ぶようにしました。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 |
/// /// setPosボタンが押されたとき private void setPosButton_Click(object sender, EventArgs e) { bool flag; flag = b3mSetPosition(serialPort1, (byte)idNumericUpDown.Value,(int)posNumericUpDown.Value,0); if (flag == false) { MessageBox.Show("データの送信に失敗しました"); } } |
ID以外にも角度を指定できるよう、NumericUpDownを配置してその値を用います。
これを実行しますが、Normalモードになっていないと動きませんので、先ほど作成したNormalモードのボタンをクリックしておきます。
IDおよび角度指定のNumericUpDownで指定された角度に動くことが確認できると思います。
現在角度を取得する
さて、目標位置へ動いたはずですが、実際に目標位置に到達したかどうかを確認するためB3Mサーボモータの位置情報の取得も行いたいと思います。
現在位置は0x2C番地から2byte分データが格納されます。
データを読み取るためには、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 |
/// /// サーボの現在角度を読み込む static public bool b3mAngleRead(SerialPort serialPort, byte servoID, ref int angle) { ByteList cmd = new ByteList(); byte[] rx = new byte[7]; //コマンド受信用(Readで2byte受け取るコマンドは7byte) //コマンドの作成 cmd.Bytes = B3MLib.B3MLib.Read(0x00, B3MLib.B3MLib.SERVO_CURRENT_POSITION, 2, servoID); //option:0 //address:B3MLib.B3MLib.SERVO_CURRENT_POSITION (0x2C) //count:2(受け取るデータの数) //ID:servoID // コマンドを送信 if (B3MLib.B3MLib.Synchronize(serialPort, cmd.Bytes, ref rx) == false) { return false; } //取得したデータをint(short)型に変換 angle = (short)Extensions.Converter.ByteConverter.ByteArrayToInt16(rx[4], rx[5]); return true; } |
2byteのデータを取得する場合、リトルエンディアンで保存されていますので、変換するときに注意が必要です。
下記は、ボタンをクリックしたときに指定したIDの現在位置を取得し、それをNumericUpDownに代入、表示を行うイベント関数です。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 |
/// /// getPosボタンが押されたとき private void getPosButton_Click(object sender, EventArgs e) { bool flag; int angle = new int(); flag = b3mAngleRead(serialPort1, (byte)idNumericUpDown.Value, ref angle); if (flag == false) { MessageBox.Show("データの送信に失敗しました"); return; } posNumericUpDown.Value = angle; } |
Freeの場合でもNormalの場合でも現在の位置が取得できるようになりました。
まとめ
今回、動作モードを変更する、指令位置を送る方法、現在位置を読み取る方法を解説いたしました。
これらをまとめると、下記のようになります。
| 動作 | コマンド | アドレス(番地) | アクセスするデータ数 |
| 動作モードを変更 | Write | 0ｘ28 | 1Byte |
| 目標位置を送る | Position | - | - |
| 現在位置を読み取る | Read | 0x2C | 2Byte |
Readコマンドを用いる事でデータを取得し、Writeコマンドでデータを書き込むことができます。
アドレスを変更することで違うデータの読み込み、書き込みができるようになります。
また、Byte数を変更することで、続いているデータの読み込みや書き込みもできるようになります。
これらを応用することで、現在の電流を取得したりゲインの変更等ができるようになります。
サンプルプログラム
最後に今回解説しました内容に関して、サンプルプログラムを公開します。
参考にしてください。
【ダウンロード】B3M_Sample_C#_20160405
2018.4.20追記 ダウンロードを再開しました。
B3M-SB-1040-Aの詳細をみる B3M-SC-1040-Aの詳細をみる B3M-SC-1170-Aの詳細をみる の詳細をみる RS485USB/シリアル変換アダプターの詳細をみる