ロボゼロのサーボ位置を読んでみる
2012年7月1日
2012年7月2日改訂


 前回はちょっとふざけたプログラムを作って音声や条件分岐のプログラムの作成方法を解説しましたが、今回はいたって真面目なプログラムです。
 前回は出力のみでロボゼロを動かすプログラムですが今回はステータスを読み取って入力を処理するプログラムです。
 入力といっても特別なセンサーを使う訳ではなくロボゼロのサーボを入力装置にしています。
 今回は作成したプログラムの解説だけです。
 プログラムの作成方法やプログラムの転送方法等は前回の記事を参考にしてください。



;手振りプログラム
;初期位置
MOVE(891,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-970,7200)
;トルクセット
V31=0
:INITLOOP
(V31=100
V31=V31+1
JUMPIF(V31,<,24,INITLOOP)
;左手のトルク0%
V00=0
V01=0
V02=0
V03=0
TORQUESET
;停止ボタンフラグ
V130=0
;手振り開始
:LOOP
V51=V457
V52=V458
V53=V459
V54=V460
;サーボ位置計算
V51=V51-V505
V52=V52-V511
V53=V53-V517
V54=V54-V523
; 右手のみ動かす
MOVE(X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,V54,V53,V52,V51,600)
;停止ボタンが押されるまで繰り返し
JUMPIF(V130,=,0,LOOP)
;トルク元に戻す
V31=0
:EXITLOOP
(V31=100
V31=V31+1
JUMPIF(V31,<,24,EXITLOOP)
TORQUESET
;ホームポジション
MOVE(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7200)

 INITLOOPとEXITLOOPはV00〜V23に100をセットする繰り返し文でロボゼロのガイドに解説されているやり方です。
 TORQUESETは実際にトルク(サーボの回転力)を設定する命令で各サーボのトルクをパーセンテージで指定します。  サーボ1〜24が変数V00〜V23に対応します。
 TORQUESETを行う前にサーボ1〜サーボ4に対応する変数V00〜V03をゼロにしています。
 これらのサーボは左手の関節のサーボ4個で自由に手で曲げられるようにトルクを全くかけないようにしておきます。

 サーボの回転角度ですがロボゼロ58号のガイドによるとV505がサーボ1、V643がサーボ24に対応する変数だと書かれています。
 ここで注目すべきは変数がいくつか飛んでいる事です。  計算すると6個置きになっています。  おそらくひとつのサーボに対して6個の変数が割り当てられているのだと思います。
 これから計算するとサーボ1〜サーボ4の位置を表す変数はV505,V511,V517,V523である事が予想されます。

 なおホームポジションの変数はV457〜V480なのでこれはそのままサーボに対応しているのだと思います。
 サーボ位置はサーボの絶対値で返されるようです。
 ロボゼロの制御はホームポジションからの相対位置で指定するのでホームポジションで相対位置に変換する必要があります。
 相対位置 = 絶対位置 - ホームポジション で計算できます。
 右のサーボは左のサーボの対称となっているため求めた相対位置に-1をかけて逆方向にする必要があります。
 すなわち 右のサーボの相対位置 = (左の絶対位置 - 左ホームポジション) × -1となります。
 プログラムでは計算処理を簡略化して = 左ホームポジション - 左の絶対位置 で計算しています。

 サーボ1〜サーボ20は動かさないのでMOVE命令の引数にXを指定しています。
 サーボ21〜サーボ24が右手の関節サーボですが対応する左手の計算結果の変数を指定しています。

 最後の600は移動時間ですがこれは0.5秒に相当します。
 ガイドによるとサーボの位置を読み取るのに500ミリ秒の遅れが生じるという事なので瞬時に動かしたとしても0.5秒遅れてしまうのでその分ゆっくりと右手のサーボを動かしています。

 以上をリモコンの●ボタンが押されるまで繰り返します。



 プログラムの実行動画です。

     

 なお今回行ったプログラムはサーボを手で動かしたり予期せぬ位置に動く場合があります。  ロボゼロサーボへの影響を伴うのであくまでも自己責任という事でお願いします。


 このプログラムに音声をつけてみました。
 折角真面目なプログラムを作ったのですが結局、また遊んでしまいました。

;手振りプログラム
;初期位置
MOVE(891,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-970,7200)
;トルクセット
V31=0
:INITLOOP
(V31=100
V31=V31+1
JUMPIF(V31,<,24,INITLOOP)
;左手のトルク0%
V00=0
V01=0
V02=0
V03=0
TORQUESET
;停止ボタンフラグ
V130=0
;エラーフラグクリア
V50=0
;手振り開始
:LOOP
V51=V457
V52=V458
V53=V459
V54=V460
;サーボ位置計算
V51=V51-V505
V52=V52-V511
V53=V53-V517
V54=V54-V523
;サーボ位置異常検出
JUMPIF(V51,>,0,ERROR)
JUMPIF(V52,<,-500,ERROR)
JUMPIF(V53,>,500,ERROR)
JUMPIF(V54,<,-500,ERROR)
;エラーフラグクリア
V50=0
; 右手のみ動かす
MOVE(X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,X,V54,V53,V52,V51,600)
;停止ボタンが押されるまで繰り返し
JUMPIF(V130,=,0,LOOP)
JUMP(EXIT)
:ERROR
;エラーフラグチェック
JUMPIF(V50,=,1,LOOP)
;エラーフラグセット
V50=1
;「痛い」音声
V00=21
V01=0
V02=0
V03=30
V04=0
SOUND
WAIT(200)
JUMP(LOOP)
:EXIT
;トルク元に戻す
V31=0
:EXITLOOP
(V31=100
V31=V31+1
JUMPIF(V31,<,24,EXITLOOP)
TORQUESET
;ホームポジション
MOVE(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7200)

 サーボの計算が終わったところでサーボの位置が異常(といってもサーボの稼働範囲ですが)を検出します。
 各サーボがある位置を超えた場合ラベルERRORにジャンプします。
 この値は計算結果ですので左手(入力側)のサーボではなくて右手(出力側)のサーボのものになります。

 異常が検出されなかったらV50に0をセットします。

 このV50はエラーのフラグとして使います。
 左手をそのままにしておくと音声が出続けますので一度音声が出力されると1にセットされ出続ける事がないようにしています。
 このフラグは:LOOPの前でクリアしています。

 :ERRORはエラーの処理部分です。
 ●ボタンで終了した場合、この部分を実行しないようにJUMP(EXIT)の命令を追加しておきます。
 :EXITラベルを従来の終了処理部分の前に入れておきます。

 音声の解説は前回の記事を参照してください。
 今回の音声ファイルは21SCR.txtにしてロボゼロに転送しています。
 エラー処理の最初でV50のチェックをしてクリアされていない時は音を出さずに繰り返しに戻ります。
 V50がクリアされている場合はセットして音声の出力処理を実行します。

 プログラムの実行動画です。

     


 なお今回行ったロボゼロのサーボギアの変更やSDカードの書き換えは、ロボゼロ本体への影響を伴うのであくまでも自己責任という事でお願いします。


サーボ・モーター・ギアの交換

 ロボゼロをいろいろいじっているうちにサーボをいくつか壊してしまいました。(上記のプログラムのせいで壊れたわけではありません)
 今回はロボゼロが無理な体制をとったためはとっさに足を抑えてしまいました。
 バキっと音がしていやな予感がしました。  電源を切ると左ヒザからしたがブラブラ状態です。  完全な骨折です。



 今まで2個のサーボを取り換えています。
 今回は足の先の方なのでサーボを取り換えるとなるとサーボの束線バンドをいくつか切らなければならず手術は全身に及びます。
 そこで今回は移植手術ではなく直接ギアを交換する事にしました。
 今回のような事を想定してサーボはロボゼロのバックナンバー(サーボの回)を何冊かとギアセットを4セット購入しています。



 しかし今回2セット目を使用するのでもう少しギアセットを購入しておく必要がありそうです。
 双葉電子のBS3395というギアセットで本来、標準品のRS304MD用ですがこれと互換のロボゼロのRS306MDにも使えます。



 左ヒザのサーボを外します。



 表蓋(銘板のある方)を開けてギアを取り出します。
 手前にあるのが今回欠けたギア(4段目)です。
 9時の部分の歯がかけています。
 他のギアは大丈夫そうですがこの際、全部取り換えました。
 前回も4段目のギアが欠けてしまったので同じセットの違うギアで複数のサーボを治療する事はできませんでした。
 ギアシャフトは大丈夫そうなので以前のものをそのまま使いました。



 まず一番大きな最終段のギアをサーボ軸を手前にして本体に差し込みます。
 このギアはベアリングが両側に付いているのでこの時、本体と表蓋の裏にベアリングがの残っていない確認してください。
 ベアリングは本体にピッタリとはまるようになっていますので少し力を入れてはめ込みます。
 続いて4段目のギアをその上に裏(内側の歯を下)に向けてセットします。



 次に3段目と1段目をセットします。
 3段目は内側の歯の部分が長いギアでこれを裏側になるようにセットします。
 1段目は一番小さなギアでモータのギアにかみ合わせます。
 この時、表側(内側の歯を上)を向けます。



 最後に残った2段目のギアを裏側(内側の歯を下)に向けてセットします。
 表蓋を被せてギアの交換は完了です。

 交換前のものにはシリコン樹脂のようなものが塗ってありました。
 ギアのかみ合わせやシャフト部分にグリスのようなものを塗った方が良いのかも知れませんが今回はそのままにしてあります。



 サーボを取り付ける前に一応テストボードで動作チェックしておきます。
 ギアを変えると原点が狂うかと思っていましたが今回もバッチリ上を向きます。
 ちゃんと最終段のギアで位置を読み取っているんでしょうね。
 サーボを元の状態に戻して無事手術成功です。

目次に戻る