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

現在、出張で東京に着ているのですが、スーツケースに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って高いのよねー。ぐぬぬ。