FC2ブログ
スポンサーサイト
カテゴリ: スポンサー広告
上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。
編集 / --.--.-- / コメント: - / トラックバック: - / PageTop↑
活動報告 No.153 公称ICS3.6未対応でもRCB-4HV経由で任意のサーボの現在角度を読み取ったり色々。。
カテゴリ: 未分類

明けましておめでとうございますぁあっす。さすがにヒュー研ブログをサボりすぎた2年のマエダです。
いや仕方ないんですよ…1月は期末期間で2月は大会ラッシュじゃないですか…。。

ま、新年の挨拶よりも言い訳が多くなりそうなのでさっさと本題に入ります。

近藤科学さんのサーボモータの通信規格に関して、ICS3.5以降はコマンドとPWMのどちらかで固定角度を決められますよね。そんなICS3.5ですが、ICS3.6は今まで主流だったICS3.5のコマンド群に加えて、任意のサーボの角度データを抽出できるコマンドが追加されました。
ICS3.5以下はどうやっていたのか明確な答えを見つけることができませんでしたが、サーボに目標値付コマンドを送った後の返り値を見たり、教示機能で読み取っていたんじゃないかと思います。

任意のサーボの角度データを抽出する技術ってのは、今まで皆さんが参加してきたであろうROBO-ONEとROBO-ONE lightから、最近新設されたROBO-ONE autoに機体を流用し易くなるものだと考えています。

あまりピンと来ないかと思いますが、ちょいと図解してみます。
まず、ROBO-ONEなど操縦者が人間の場合、機体とモーション、操作母体の関係は以下の通りです。

スライド1



そして、ROBO-ONE autoなど操縦権限がコンピュータの場合、機体とモーション、操作母体の関係は以下の通りです。


スライド2



 操作母体のやっていることは、「戦況によって限られたモーションデータのどれを再生するか判断している」です。条件分岐と言うことにすると、例えば、相手がちょいと遠ければ近づいて攻撃しよう、とか人間にも明示的ではないもののある程度の条件分岐を試合中にやっています。

ROBO-ONEからROBO-ONE autoに機体を流用しないとなったら、外界認識システムから基板、モーションの分岐、そもそもの機体に至るまでイチから機体を作り直すしかありません。しかし、もしもコンピュータからコントローラのコマンドを送れたらどうでしょうか。コンピュータは自律するコントローラと同等とみなせるので、ROBO-ONEの機体にCOM端子と電源だけ繋いで即ROBO-ONE auto仕様にすることにが出来てしまいます。

上記を理由に、コンピュータからRCB-4HVに適切なコントローラのボタンデータを送ったり、条件分岐するために任意のサーボの角度データを抽出したり指定したりできることは、auto用処理モジュールを製作するのに非常に有用なのです。



使用機器:

  • ラズパイ 3B
  • KRS-2542HV
  • RCB-4HV
  • Serial USB Adapter HS


です。USB経由でラズパイからRCB-4HVにコマンドを送信して、任意のサーボの角度データを持ってきてもらったり指定してあげたりします。



準備


ラズパイに近藤科学製のシリアルUSBを接続して、利用可能な状態にする必要があります。

近藤科学は以下の記事にてLinuxでシリアルUSBを利用する方法を紹介していますが、これではできません。注意です。

http://kondo-robot.com/faq/usb-adapter-for-linux


上記サイトは2010年に書かれたものです。今は以下の記事を参考にセットアップすれば一応使えます。

https://gpsnmeajp.blogspot.jp/2015/07/ftdi-ft232rlusbraspberry-pi.html


root権限じゃないと通らないです。スクリプト作って起動時に実行するようにすれば良いでしょう。



角度を指定する


コマンド方式で指定していきます。

コマンドのルールは以下の通りです。


[0x07, 0x0F, ICS番号, フレーム数, 角度データ(16進数で)下位, 角度データ(16進数で)上位, これ以前の総和]


1つめ:0x07

固定です。変える必要はありません。


2つめ:0x0F

ここも固定です。変える必要はありません。


3つめ:ICS番号

SIO端子が1〜4番ピンに刺さっていれば、「サーボID番号 × 2」がICS番号です。

SIO端子が5〜8番ピンに刺さっていれば、「サーボID番号 × 2 + 1」がICS番号です。


4つめ:フレーム数

まんまフレーム数です。1とか30とか60とか。。


5つめ・6つめ:角度データ(16進数で)下位・上位

近藤科学の定めるサーボの角度データは


35007500(ニュートラル)〜11500


です。これらを16進数で表すと、


0DAC1D4C(ニュートラル)〜2CEC


です。真ん中を例にとると、


角度データ(16進数で)下位 = 4C

角度データ(16進数で)上位 = 1D


です。言葉でいうと、近藤科学規格のサーボポジション数値の16進数版を前半と後半で2文字ずつ分けたとき、それぞれ上位と下位になります。


7つめ:これ以前の総和

1つめ〜6つめの数値を全て足し合わせたものです。


例:

例を示します。

・RCB-4HVの1〜4番ピンのどれかにサーボを接続してある

・サーボのIDは0番

・フレーム数は30

・サーボの目標位置は近藤規格で3500(-135度)

のとき、

[0x07, 0x0F, 0x00, 0x1E, 0xAC, 0x0D, 0xED]

というコマンドを送信すればサーボが3500の位置に動いてくれます。「0x」は16進数であることを示しています。ちなみに10進数で表すと、

[7, 15, 0, 30, 172, 13, 237]

です。

[0x07, 0x0F, ICS番号, フレーム数, 角度データ(16進数で)下位, 角度データ(16進数で)上位, これ以前の総和]

と比較してみてください。



ほんで、以下のプログラムをゴリゴリ書いていきます。Python3.5.3推奨です。ちなみにまだ未完成ですので、コピペは後でお願いします。


↓ 横スクロールできます ↓ ※ まだ未完成

import serial
from time import sleep


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
else:
return False

# 普通の角度か近藤規格の角度データかチェック
if -135 <= deg <= 135:
kondo_deg = 800.0 * deg / 27.0 + 7500.0
elif 3500 <= deg <= 11500:
kondo_deg = deg
else:
return False

deg_16 = hex(int(kondo_deg)) # 16進数に変換
deg_16 = deg_16[2:].zfill(4) # 0x以降を抽出して0パディング
deg_16_H, deg_16_L = int(deg_16[:2], 16), int(deg_16[2:], 16) # 上位と下位で分割
tail = 0x07 + 0x0F + ics + frames + deg_16_L + deg_16_H # 送信するコマンドの末尾に必要らしい(用途不明)
com_line = [0x07, 0x0F, ics, frames, deg_16_L, deg_16_H, tail] # 完成!!

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秒待っています。


動いてくれたら嬉しいっス。。


しかしバグ発生!!


先ほど送信コマンドのルールとして


[0x07, 0x0F, ICS番号, フレーム数, 角度データ(16進数で)下位, 角度データ(16進数で)上位, これ以前の総和]


と紹介しましたが、これの7要素目の「これ以前の総和」が256以上になると16進法で2バイトになってしまうので送信できませんでした。分割してもダメです。

RCB-4リファレンスを読み返してみると、


「チェックサム: 1byte⽬のサイズからチェックサムの1byte⼿前までのデタを加算した下位1byte」


と書いてありました。

というわけで、10進法で256以上の場合は下位1byteだけを抜き出してみます。


先ほどのset_pos関数を以下に書き換えます。


 
def set_pos(ser, sio_pin, servo_id, frames, deg):
""" 任意の1つのサーボをフレーム数と角度を指定して動かす """
# sioピンのチェック
if 1 <= sio_pin <= 4:
ics = servo_id * 2
elif 5 <= sio_pin <= 8:
ics = servo_id * 2 + 1
else:
return False

# 普通の角度か近藤規格の角度データかチェック
if -135 <= deg <= 135:
kondo_deg = 800.0 * deg / 27.0 + 7500.0
elif 3500 <= deg <= 11500:
kondo_deg = deg
else:
return False

deg_16 = hex(int(kondo_deg)) # 16進数に変換
deg_16 = deg_16[2:].zfill(4) # 0x以降を抽出して0パディング
deg_16_H, deg_16_L = int(deg_16[:2], 16), int(deg_16[2:], 16) # 上位と下位で分割
com_line = [0x07, 0x0F, ics, frames, deg_16_L, deg_16_H]

tail = 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))
else:
com_line.append(tail)

ser.write(com_line)
_ = ser.readline()

return True




角度を読み取る


角度を読み取るときに使う特別なコマンドのルールはありません。


手順としては、まずHeartToHeart4のモーション作成パネルにて、「GetValue」を引っ張り出してきます。


キャプチャ



GetValueのブロックをダブルクリックしたら、以下の画面のように設定します。


キャプチャ2



現在値読み取りで返り値をCOMポートに出力します。ラズパイは4HVから返ってくる値を読み取れば良いだけです。


サーボの現在位置は人によって違うと思うので、ここは適宜対応お願いします。


設定したら、一番下にコマンドが出てくるので、メモしておきます。これが任意のサーボの角度を読み取るコマンドになります。


Python側のプログラムは関数だけ見せると以下になります。



↓ 横スクロールできます ↓

from time import sleep

def get_pos(ser, com_line, krs_deg=False):
""" コントローラに配置されたボタンを用いて任意のサーボの現在角度を読み取る """
for _ in range(100):
sleep(0.01)
ser.write(com_line)
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 kondo_deg if krs_deg is True else int(0.03375 * (kondo_deg - 7500))

return None



ざっとこんな感じです。0.01秒間隔で100回のループを行なっているのは、角度によって値が返ってくるまでの時間にばらつきがあるからです。



引用: http://kondo-robot.com/faq/ics3_6function

kondo.png 

上のグラフは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の間で返ってきます。



今まで紹介してきた関数群の使用例は以下の通りです。


↓ 横スクロールできます ↓

import serial
from time import sleep


def set_pos(ser, sio_pin, servo_id, frames, deg):
""" 任意の1つのサーボをフレーム数と角度を指定して動かす """
# sioピンのチェック
if 1 <= sio_pin <= 4:
ics = servo_id * 2
elif 5 <= sio_pin <= 8:
ics = servo_id * 2 + 1
else:
return False

# 普通の角度か近藤規格の角度データかチェック
if -135 <= deg <= 135:
kondo_deg = 800.0 * deg / 27.0 + 7500.0
elif 3500 <= deg <= 11500:
kondo_deg = deg
else:
return False

deg_16 = hex(int(kondo_deg)) # 16進数に変換
deg_16 = deg_16[2:].zfill(4) # 0x以降を抽出して0パディング
deg_16_H, deg_16_L = int(deg_16[:2], 16), int(deg_16[2:], 16) # 上位と下位で分割
com_line = [0x07, 0x0F, ics, frames, deg_16_L, deg_16_H]

tail = 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))
else:
com_line.append(tail)

ser.write(com_line)
_ = ser.readline()

return True


def get_pos(ser, com_line, krs_deg=False):
""" コントローラに配置されたボタンを用いて任意のサーボの現在角度を読み取る """
for _ in range(100):
sleep(0.01)
ser.write(com_line)
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 kondo_deg if krs_deg is True else int(0.03375 * (kondo_deg - 7500))

return None


def main():
ser = serial.Serial('/dev/ttyUSB0', 115200, parity=serial.PARITY_EVEN, timeout=1)
print('シリアルポートを開きました')

# 角度を読み取るコマンド(HTH4GetValueから要確認)
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))

ser.close()
print('シリアルポート閉じました')


if __name__ == '__main__':
main()



set_pos関数とget_pos関数の間にsleepを入れています。今は適当ですが、sleepをいい感じで入れないと、サーボが目標値に達する前に読み取りが呼び出されます。


一応set_pos関数内にser.readline()を入れてみたんですが、非ブロッキングなのかな?返り値が来るまで止められないっぽいです。


set_pos関数の仮引数にタイムアウトを置きつつ、返り値が来るまでwhile文で回して、返ってきたらブレイクするのがいいのかなぁ、と思ったり。。



おわり


てなわけで今回は公称ICS3.6未対応でもRCB-4HV経由で任意のサーボの現在角度を読み取ったり角度を指定したりをPythonで制御する記事でした。







スポンサーサイト
編集 / 2018.02.11 / コメント: 0 / トラックバック: 0 / PageTop↑
コメント
 
Title
 
 
 
 
 
 
Secret 


Pagetop↑
トラックバック
Pagetop↑
プロフィール

ヒュー研の中の人

Author:ヒュー研の人
このブログは東京電機大学理工学部ヒューマノイド研究部の公式ブログです。2012年から部に昇格しました!
その日の活動や大会の記録をできるだけ更新していきたいです!!

☆だいたい金曜日前後に更新します☆

FC2カウンター
カレンダー
09 | 2018/10 | 11
- 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 27
28 29 30 31 - - -
リンク
ブロとも申請フォーム
携帯でみるには↓
QR
上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。