Visual Basic Express Editironで
コマンド方式サーボを動かしてみる(5)
|
(4)へ
(6)へ
7.サーボのデータ取得
7.1.動作の内容
コマンド方式サーボってのは双方向通信できるんです!
一応VBでも出来たんですが、ちょっと怪しいところがあるのであまり信じない方がいいかもしれませんが・・・
データを受信するためには、まずサーボにデータを返信するよう指示を出します。
このとき、サーボのメモリマップ中のどの部分を返信して欲しいのかも指定します。
受信要求を受け取ったサーボは、指定されたデータを返信します。
この返信されるデータが取説などで”リターンパケット”と呼ばれるものになります。
※2011/2/17 編集
いろいろ試したところ、何とかなりそうなデータの取り方ができましたので内容変更します。
7.2.入力欄の配置と初期設定の追加今までとは内容が違いますので、改めて画面作り直してみます。
新規に別プロジェクト作っちゃった方が速いと思います。
画面イメージは↓こんな感じ。

COMポートとサーボのIDはコンボボックスで選択できるようにしました。
いちいち入力するのが面倒なので・・・
プロパティの【Item】には、それぞれ次のように入力しています。
COMポートは意外と10以上になることが多いので、20まで。
テキストをそのままシリアルポートのPortNameに利用するので、COMxという書式にしています。

IDは1~127までと、255を選択できるようにします。
255は全サーボへの共通命令になります。
トルクONボタンとトルクOFFボタンについては、先に紹介したものと全く同じです。
通信設定間違っていないか確認するためにつけただけのものなので、別に無くても問題はありません。
【TextBox1】(取得開始アドレス)と【TextBox2】(取得バイト数)は、
何も入力が無いとあとで困るのでプロパティの【Text】に【1】を入れときます。
何がどう困るかは、後で詳しく。。
それと、SerialPortの設定も一部変更します。
ここ重要ですので、変更を忘れずに!

変更するのは【ReadTimeout】の項目。
デフォルトでは”-1”になっていると思いますが、これを"200”くらいに設定してください。
以上で環境の準備は完了です。
SerialPortのReadTimeoutは重要なので、変更をお忘れなく。
7.3.リターン要求パケットの作成
取得アドレス と 取得バイト数は、取得するデータの内容設定に使用します。
サーボにリターンデータを要求するためのパケットにはいくつか種類があるのですが、
ここでは”指定アドレスから指定バイト数受信する”方法を試してみます。
なので
「IDxxのサーボのメモリマップアドレスNo.YYからZバイト分のデータを寄越せ」
というパケットを作成します。
まぁ、丸ごとコピペ用にソースは一気に書いちゃいますが。
Private Sub Button3_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Button3.Click
Dim ReturnRequest(20) As Byte
Dim ReturnPacket(20) As Byte
Dim i,j, pID, pGetAddress, pGetLength, sum As Integer
'リターン要求パケット作成
pID = CInt(ComboBox2.Text)
pGetAddress = CInt(TextBox1.Text)
pGetLength = CInt(TextBox2.Text)
ReturnRequest(0) = &HFA
ReturnRequest(1) = &HAF
ReturnRequest(2) = pID
ReturnRequest(3) = &HF
ReturnRequest(4) = pGetAddress
ReturnRequest(5) = pGetLength
ReturnRequest(6) = &H0
sum = ReturnRequest(2)
For i = 3 To 6
sum = sum Xor ReturnRequest(i)
Next
ReturnRequest(7) = sum
Try
SerialPort1.Open()
Catch ex As Exception
MessageBox.Show("シリアルポートエラー")
End Try
If SerialPort1.IsOpen = True Then
SerialPort1.Write(ReturnRequest, 0, 8) 'リターンデータ要求パケット送信
j=0
Do until Serialport1.BytesToRead = (7+ pGetLength + 1) OR J>100
j = j +1
Loop
if j > 100 then
SerialPort1.Close()
End Sub
End If
SerialPort1.Read(ReturnPacket, 0, 7+pGetLength + 1)
SerialPort1.Close()
End If
TextBox3.Text = Hex(ReturnPacket(0)) '取得したデータの表示
TextBox4.Text = Hex(ReturnPacket(1))
TextBox5.Text = Hex(ReturnPacket(2))
TextBox6.Text = Hex(ReturnPacket(3))
TextBox7.Text = Hex(ReturnPacket(4))
TextBox8.Text = Hex(ReturnPacket(5))
TextBox9.Text = Hex(ReturnPacket(6))
TextBox10.Text = Hex(ReturnPacket(7))
TextBox11.Text = Hex(ReturnPacket(8))
TextBox12.Text = Hex(ReturnPacket(9))
TextBox13.Text = Hex(ReturnPacket(10))
End Sub
毎度のことながら変数名は適当です。
”ReturnRequest(20)”がリターンパケット要求用のショートパケットで、
”ReturnPacket(20)”が受信したパケットの収納先になります。
pID = CInt(ComboBox2.Text)
pGetAddress = CInt(TextBox1.Text)
pGetLength = CInt(TextBox2.Text)
IDと取得開始アドレス、取得データ長を入力欄から取得します。
ReturnRequest(0) = &HFA
ReturnRequest(1) = &HAF
ReturnRequest(2) = pID
ReturnRequest(3) = &HF
ヘッダーとIDはこれまでのショートパケットと同じ。
フラグはリターンパケットの要求のため、0xFHになってます。
詳細はコマンド方式サーボの取扱説明書、ショートパケットの説明箇所をご参照ください。
ReturnRequest(4) = pGetAddress
ReturnRequest(5) = pGetLength
ReturnRequest(6) = &H0
IDと取得開始アドレス、取得データ長を入力欄から取得します。
sum = ReturnRequest(2)
For i = 3 To 6
sum = sum Xor ReturnRequest(i)
Next
ReturnRequest(7) = sum
Try
SerialPort1.Open()
Catch ex As Exception
MessageBox.Show("シリアルポートエラー")
End Try
チェックサムの計算と、シリアルポートの通信確認はこれまでと同じなので説明は割愛。
7.4.リターンパケットの取得(修正版)
SerialPort1.Write(~)でリターンパケットを要求、
その後SerialPort1.Read(~)で返信されたリターンパケットを受信するわけですが、
どういうわけか一回目はリターンパケットの1バイト目しか取得できませんでした。
その後続けてもう一度受信処理をすると、2バイト目以降が取得されます。
どうやらリターンパケットが届くまでの時間が結構かかったり環境等により遅くなったりするようです。
Wait関数作って待機してても読めるんですが、別の方法で解決してみました。
If SerialPort1.IsOpen = True Then
SerialPort1.Write(ReturnRequest, 0, 8) 'リターンデータ要求パケット送信
j=0
Do until Serialport1.BytesToRead = (7+ pGetLength + 1) OR J>100
j = j +1
Loop
if j > 100 then
SerialPort1.Close()
End Sub
End If
SerialPort1.BytesToRead コマンドを使うと、受信バッファに溜まってるデータのサイズが判ります。
リターンパケットはヘッダー~カウントまでの7バイト+指定したデータ長+チェックサム1バイトで返ってきますので、受信バッファの中身がそのサイズになるまで繰り返し確認する、という作業になります。
ただしこれだと受信データのサイズ指定間違ってたり返信が無かったりした場合に無限ループになっちゃいますので、カウント入れて指定回数以上繰り返したときに脱出するようにしています。
これでループを抜けた場合は何か異常があるということですので、シリアルポート閉じて終了しちゃいます。
SerialPort1.Read(ReturnPacket, 0, 7+pGetLength + 1)
SerialPort1.Close()
End If
リターンパケットの要求の際にCOMポート開いてますので、ここではTry~の内容は省略。
SerialPort1.BytesToReadのところで受信すべきデータが溜まってるのは確認済なので、あとは必用なバイト数分受け取るだけ。
データを受け取り終わったらシリアルポート閉じておきます。
TextBox3.Text = Hex(ReturnPacket(0)) '取得したデータの表示
TextBox4.Text = Hex(ReturnPacket(1))
TextBox5.Text = Hex(ReturnPacket(2))
TextBox6.Text = Hex(ReturnPacket(3))
TextBox7.Text = Hex(ReturnPacket(4))
TextBox8.Text = Hex(ReturnPacket(5))
TextBox9.Text = Hex(ReturnPacket(6))
TextBox10.Text = Hex(ReturnPacket(7))
TextBox11.Text = Hex(ReturnPacket(8))
TextBox12.Text = Hex(ReturnPacket(9))
TextBox13.Text = Hex(ReturnPacket(10))
End Sub
というわけで、読み込んだデータをTextBoxに入れます。
取説上の表記が16進法なので
うまくいけば、データ表示されてくれるはず、ということで実行してみましょう。
7.5.動作確認
実際に起動して、試してみましょう。
COMポートを設定したら、トルクON/OFFでもってちゃんと通信ができるかどうか確認します。
また、どういうデータが取得できるのが適切なのかは取扱説明書で確認してください。 まずはNo.4から1バイト=IDを取得。

ID1のサーボのIDを読み込んでるんだから、当然1が出ます。
Dataの1バイト目が取得した情報の”1”でその次の”4”はリターンパケットのチェックサムです。

これはNo.35の1バイト=最大トルクを取得したところ。
初期値のまま変えていないので64H(100%)が表示されています。
ちょっと胡散臭いところがありますが、ともあれこれでデータの読み込みができるようになりました。
今回はここまで。 |