Excel VBAでG-Robotsを制御する覚書

Tweet

現在、出張で東京に着ているのですが、スーツケースにG-Robotsを忍ばせてきているので、講習会終わった後はラーメン屋めぐりしつつ、ホテルではまったりプログラミングしてたりします。

出張前にKO-LINK2のエクセルでの歩行シミュレーターにジョイスティックを対応させてみたのだけれど、エクセルからサーボも制御できればもっと面白いことが出来るはず・・・。
ただ、Dynamixelを制御マイコンのCM-5からスルーパケットで制御するためのライブラリがないので（作れば良いのだけれど）…
というわけで、ライブラリが公開されているG-Robotsを使ってエクセルで制御してみました。

ライブラリがRSC-U485用なので、RロボのRPU-11用に書き直しました。その改造方法を覚書として残します。
（今日のツイートでUE村さんも作られたみたいなので、出来ればFUTABAのサイトでスマートな公式版を出していただけると非常にありがたいですけど。）

—

さて基本的にFUTABAの「RSC-U485用サンプル for Microsoft Excel」をベースに改造します。改造点は３か所。

１．標準モジュール1内のsrvSendを改造

' サーボ制御用関数
' このコマンドを使用する場合には、通信ポートをオープンしておく必要がある
' 送信用する
'
Public Function srvSend(ByVal pCom As Long, ByVal pID As Integer, ByVal pFlag As Integer, ByVal pAddress As Integer, ByVal pCount As Integer, ByVal pLength As Integer, ByVal pDataLen As Integer, ByRef pData() As Byte) As Integer
Dim myBuf(512) As Byte
Dim mySum As Byte
Dim myRet As Long
Dim i As Integer
' 送信バッファを作成する
myBuf(0) = &H53                 ' 0x53
myBuf(1) = (pLength + 8) And &HFF
myBuf(2) = &HFA                 ' 0xFA
myBuf(3) = &HAF                 ' 0xAF
myBuf(4) = pID And &HFF         ' ID
myBuf(5) = pFlag And &HFF       ' Flag
myBuf(6) = pAddress And &HFF    ' Address
myBuf(7) = pLength And &HFF     ' Length
myBuf(8) = pCount And &HFF      ' Count
For i = 0 To (pDataLen - 1)
myBuf((i + 9)) = pData(i)
Next i
' SUMの計算
mySum = myBuf(4)
For i = 5 To (pDataLen + 8)
mySum = mySum Xor myBuf(i)
Next i
myBuf((pDataLen + 9)) = mySum
' 送信する
myRet = WriteComm(pCom, (pDataLen + 10), myBuf)
srvSend = myRet
End Function

２．受信用に標準モジュール1内にsrvSend2を追記

' サーボ制御用関数2(リターンパケット用)
' このコマンドを使用する場合には、通信ポートをオープンしておく必要がある
' 送信用する
'
Public Function srvSend2(ByVal pCom As Long, ByVal pID As Integer, ByVal pFlag As Integer, ByVal pAddress As Integer, ByVal pCount As Integer, ByVal pLength As Integer, ByVal pDataLen As Integer, ByRef pData() As Byte) As Integer
Dim myBuf(512) As Byte
Dim mySum As Byte
Dim myRet As Long
Dim i As Integer
' 送信バッファを作成する
myBuf(0) = &H54                 ' 0x54
myBuf(1) = (pLength + 9) And &HFF
myBuf(2) = &HFA                 ' 0xFA
myBuf(3) = &HAF                 ' 0xAF
myBuf(4) = pID And &HFF         ' ID
myBuf(5) = pFlag And &HFF       ' Flag
myBuf(6) = pAddress And &HFF    ' Address
myBuf(7) = pLength And &HFF     ' Length
myBuf(8) = pCount And &HFF      ' Count
For i = 0 To (pDataLen - 1)
myBuf((i + 9)) = pData(i)
Next i
' SUMの計算
mySum = myBuf(4)
For i = 5 To (pDataLen + 8)
mySum = mySum Xor myBuf(i)
Next i
myBuf((pDataLen + 9)) = mySum
myBuf((pDataLen + 10)) = &H10
' 送信する
myRet = WriteComm(pCom, (pDataLen + 11), myBuf)
srvSend2 = myRet
End Function

３．受信用に標準モジュール2内のServoGetDataを下記の通り改変

' サーボのデータを取得する
' 別途、通信用のポートをオープンしておく必要がある
' ビット演算がVBAにはない；；
' pCom      ：通信ポート
' pID       ：サーボID
'
Public Function ServoGetData(ByVal pCom As Long, ByVal pID As Integer, ByVal pFlag As Integer, ByRef pData() As Byte) As Long
Dim mySend As Long
Dim myRead As Long
Dim myID As Integer
Dim myFlag As Integer
Dim myAddress As Integer
Dim myCount As Integer
Dim myLength As Integer
Dim myBuf(256) As Byte
' 引数設定
myID = pID
myFlag = pFlag          ' 通常は9
myAddress = 0
myCount = 1
myLength = 0
myBuf(0) = 0
myBuf(1) = 0
' データを送信する
mySend = srvSend2(pCom, myID, myFlag, myAddress, myCount, myLength, myLength, myBuf)
' データの読み込み
myRead = ReadComm(pCom, 48, pData)
' 戻りを設定
ServoGetData = myRead
End Function

なんかかなり簡単に動いたので拍子抜け。
この勢いでGロボの逆運動学のエクセルシートも作ってみようと思う。

ただ、なぜか大腿部のサーボが焼けてしまったのでとりあえず帰ってから修理かな。
予備のサーボないのでションボリ。RS301CRって高いのよねー。ぐぬぬ。