明けましておめでとうございますぁあっす。さすがにヒュー研ブログをサボりすぎた2年のマエダです。
いや仕方ないんですよ…1月は期末期間で2月は大会ラッシュじゃないですか…。。
ま、新年の挨拶よりも言い訳が多くなりそうなのでさっさと本題に入ります。
近藤科学さんのサーボモータの通信規格に関して、ICS3.5以降はコマンドとPWMのどちらかで固定角度を決められますよね。そんなICS3.5ですが、ICS3.6は今まで主流だったICS3.5のコマンド群に加えて、任意のサーボの角度データを抽出できるコマンドが追加されました。
ICS3.5以下はどうやっていたのか明確な答えを見つけることができませんでしたが、サーボに目標値付コマンドを送った後の返り値を見たり、教示機能で読み取っていたんじゃないかと思います。
任意のサーボの角度データを抽出する技術ってのは、今まで皆さんが参加してきたであろうROBO-ONEとROBO-ONE lightから、最近新設されたROBO-ONE autoに機体を流用し易くなるものだと考えています。
あまりピンと来ないかと思いますが、ちょいと図解してみます。
まず、ROBO-ONEなど操縦者が人間の場合、機体とモーション、操作母体の関係は以下の通りです。
そして、ROBO-ONE autoなど操縦権限がコンピュータの場合、機体とモーション、操作母体の関係は以下の通りです。
操作母体のやっていることは、「戦況によって限られたモーションデータのどれを再生するか判断している」です。条件分岐と言うことにすると、例えば、相手がちょいと遠ければ近づいて攻撃しよう、とか人間にも明示的ではないもののある程度の条件分岐を試合中にやっています。
ROBO-ONEからROBO-ONE autoに機体を流用しないとなったら、外界認識システムから基板、モーションの分岐、そもそもの機体に至るまでイチから機体を作り直すしかありません。しかし、もしもコンピュータからコントローラのコマンドを送れたらどうでしょうか。コンピュータは自律するコントローラと同等とみなせるので、ROBO-ONEの機体にCOM端子と電源だけ繋いで即ROBO-ONE auto仕様にすることにが出来てしまいます。
上記を理由に、コンピュータからRCB-4HVに適切なコントローラのボタンデータを送ったり、条件分岐するために任意のサーボの角度データを抽出したり指定したりできることは、auto用処理モジュールを製作するのに非常に有用なのです。
使用機器：
です。USB経由でラズパイからRCB-4HVにコマンドを送信して、任意のサーボの角度データを持ってきてもらったり指定してあげたりします。
準備
ラズパイに近藤科学製のシリアルUSBを接続して、利用可能な状態にする必要があります。
近藤科学は以下の記事にてLinuxでシリアルUSBを利用する方法を紹介していますが、これではできません。注意です。

上記サイトは2010年に書かれたものです。今は以下の記事を参考にセットアップすれば一応使えます。
root権限じゃないと通らないです。スクリプト作って起動時に実行するようにすれば良いでしょう。
角度を指定する
コマンド方式で指定していきます。
コマンドのルールは以下の通りです。
*[0x07, 0x0F, ICS番号, フレーム数, 角度データ(16進数で)下位, 角度データ(16進数で)上位, これ以前の総和]* 1つめ：0x07
固定です。変える必要はありません。
２つめ：0x0F
ここも固定です。変える必要はありません。
３つめ：ICS番号
SIO端子が１〜４番ピンに刺さっていれば、「サーボID番号 × 2」がICS番号です。
SIO端子が５〜８番ピンに刺さっていれば、「サーボID番号 × 2 + 1」がICS番号です。
４つめ：フレーム数
まんまフレーム数です。1とか30とか60とか。。
５つめ・６つめ：角度データ(16進数で)下位・上位
近藤科学の定めるサーボの角度データは
です。これらを16進数で表すと、
0DAC〜1D4C(ニュートラル)〜2CEC
です。真ん中を例にとると、
角度データ(16進数で)下位 = 4C
角度データ(16進数で)上位 = 1D
です。言葉でいうと、近藤科学規格のサーボポジション数値の16進数版を前半と後半で2文字ずつ分けたとき、それぞれ上位と下位になります。
７つめ：これ以前の総和
１つめ〜６つめの数値を全て足し合わせたものです。
例：
例を示します。
・RCB-4HVの1〜4番ピンのどれかにサーボを接続してある
・サーボのIDは0番
・フレーム数は30
・サーボの目標位置は近藤規格で3500(-135度)
のとき、
*[0x07, 0x0F, 0x00, 0x1E, 0xAC, 0x0D, 0xED]*
というコマンドを送信すればサーボが3500の位置に動いてくれます。「0x」は16進数であることを示しています。ちなみに10進数で表すと、
です。
*[0x07, 0x0F, ICS番号, フレーム数, 角度データ(16進数で)下位, 角度データ(16進数で)上位, これ以前の総和]*
と比較してみてください。
ほんで、以下のプログラムをゴリゴリ書いていきます。Python3.5.3推奨です。ちなみにまだ未完成ですので、コピペは後でお願いします。
↓ 横スクロールできます ↓
def set_pos(ser, sio_pin, servo_id, frames, deg):
# sioピンのチェック
if 1 <= sio_pin <= 4:
ics = servo_id * 2
elif 5 <= sio_pin <= 8:
ics = servo_id * 2 + 1
# 普通の角度か近藤規格の角度データかチェック
if -135 <= deg <= 135:
kondo_deg = 800.0 * deg / 27.0 + 7500.0
elif 3500 <= deg <= 11500:
kondo_deg = deg
deg_16 = hex(int(kondo_deg)) # 16進数に変換
deg_16 = deg_16[2:].zfill(4) # 0x以降を抽出して0パディング
tail = 0x07 + 0x0F + ics + frames + deg_16_L + deg_16_H # 送信するコマンドの末尾に必要らしい(用途不明)
ser.write(com_line)
sleep(2) # どう設定しようか。。
return True
def main():
ser = serial.Serial('/dev/ttyUSB0', 115200, parity=serial.PARITY_EVEN, timeout=1)
print('シリアルポートを開きました')
_ = set_pos(ser, 1, 0, 30, 0)
ser.close()
print('シリアルポート閉じました')
if __name__ == '__main__':
main()
set_pos関数にはPySerialでシリアル接続した際の返り値とsioピンの番号、サーボのID、フレーム、角度が引数として必要です。
上の例ではsioピン1番に接続されたID0のサーボをフレーム数30でニュートラルの位置まで動かしています。
-135 ~ 135の間で指定すれば自動的に単位が[度]として処理されますし、3500 ~ 11500の間であれば近藤規格のパラメータで処理されます。
set_pos関数の最後の方にあるsleepでは小数点使用可能で秒単位の待ち時間が指定できますが、どれくらい待てば良い
のやら。。例では適当にとりあえず2秒待っています。
動いてくれたら嬉しいっス。。
しかしバグ発生！！
先ほど送信コマンドのルールとして
と紹介しましたが、これの7要素目の「
*これ以前の総和*」が256以上になると16進法で2バイトになってしまうので送信できませんでした。分割してもダメです。
RCB-4リファレンスを読み返してみると、
「チェックサム: 1byte⽬のサイズからチェックサムの1byte⼿前までのデタを加算した下位1byte」
と書いてありました。
というわけで、10進法で256以上の場合は下位1byteだけを抜き出してみます。
先ほどのset_pos関数を以下に書き換えます。
""" 任意の1つのサーボをフレーム数と角度を指定して動かす """
com_line = [0x07, 0x0F, ics, frames, deg_16_L, deg_16_H]
if tail >= 256:
# 10進数でいう256以上は16進数で2byteになるけど，下位を採用して切り抜ける
tail_16 = hex(tail)
com_line.append(int(tail_16[-2:], 16))
com_line.append(tail)
_ = ser.readline()
角度を読み取る
角度を読み取るときに使う特別なコマンドのルールはありません。
手順としては、まずHeartToHeart4のモーション作成パネルにて、
「GetValue」を引っ張り出してきます。
GetValueのブロックをダブルクリックしたら、以下の画面のように設定します。
現在値読み取りで返り値をCOMポートに出力します。ラズパイは4HVから返ってくる値を読み取れば良いだけです。
サーボの現在位置は人によって違うと思うので、ここは適宜対応お願いします。
設定したら、一番下にコマンドが出てくるので、メモしておきます。これが任意のサーボの角度を読み取るコマンドになります。
Python側のプログラムは関数だけ見せると以下になります。
from time import sleepdef get_pos(ser, com_line, krs_deg=False):
""" コントローラに配置されたボタンを用いて任意のサーボの現在角度を読み取る """
for _ in range(100):
sleep(0.01)
data = ser.readline() # バイト列で受信
r_data = data.hex() # バイト列から16進数に変換
'''
r_data[0:2] = '05': 0x05はサーボの角度読み取り返り値を示す
r_data[2:4] = サーボモータのID番号 <- 使っていないけど一応
r_data[6:8] + r_data[4:6] = 角度データ(3500 ~ 7500:ニュートラル ~ 11500)
if r_data[0:1] is '0' and r_data[1:2] is '5':
deg_16 = r_data[6:8] + r_data[4:6]
kondo_deg = int(deg_16, 16) # 16進数から10進数に変換
return None
ざっとこんな感じです。0.01秒間隔で100回のループを行なっているのは、角度によって値が返ってくるまでの時間にばらつきがあるからです。
引用: 
上のグラフはICS3.6で角度を読み取るコマンドを発行したのち返ってくるまでの時間経過を表しています。
今回は4HV経由で指令を出していますが、上と同様なことが言えます。角度によって得たい数値が返ってくるまでにラグがほとんどなかったり0.5秒以上あったりです。なので、そこのばらつきを関数内にてfor文とsleepを使って吸収しています。
get_pos関数の引数に
krs_deg=Falseとありますが、これは返り値が近藤規格の角度データにするか否かを設定します。デフォルトのFalseであれば-135 ~ 135度の間で返ってきますし、Trueにすれば3500 ~ 11500の間で返ってきます。
get_pos関数は自律制御に使うカメラの角度に使おうとしているので、使用目的に合わなければ関数が宣言されている行の引数を
krs_deg=Trueとしてもらえればデフォルトで3500 ~ 11500の間で返ってきます。
今まで紹介してきた関数群の使用例は以下の通りです。
def get_pos(ser, com_line, krs_deg=False):
# 角度を読み取るコマンド(HTH4のGetValueから要確認)
get_com_line = [0x0A, 0x00, 0x20, 0x00, 0x00, 0x00, 0x96, 0x00, 0x02, 0xC2]
_ = set_pos(ser, 1, 0, 30, 135) # 指定角度に固定
sleep(1)
d = get_pos(ser, get_com_line, krs_deg=False) # 現在角度検出
print('検出角度： ' + str(d))
set_pos関数とget_pos関数の間にsleepを入れています。今は適当ですが、sleepをいい感じで入れないと、サーボが目標値に達する前に読み取りが呼び出されます。
一応set_pos関数内にser.readline()を入れてみたんですが、非ブロッキングなのかな？返り値が来るまで止められないっぽいです。
set_pos関数の仮引数にタイムアウトを置きつつ、返り値が来るまでwhile文で回して、返ってきたらブレイクするのがいいのかなぁ、と思ったり。。
おわり
てなわけで今回は公称ICS3.6未対応でもRCB-4HV経由で任意のサーボの現在角度を読み取ったり角度を指定したりをPythonで制御する記事でした。
Author:ヒュー研の人
このブログは東京電機大学理工学部ヒューマノイド研究部の公式ブログです。2012年から部に昇格しました！
その日の活動や大会の記録をできるだけ更新していきたいです！！
☆だいたい金曜日前後に更新します☆