RPU-10講座/第九章 モーションっぽくサーボ動作

公開日:  最終更新日:2014/06/05

すいません m(_ _)m、本記事はブログ引越時に書式が崩れました。順次修正中です。

それでは、引き続きRPU-10講座、今回はサーボをモーションっぽく動作させます。今回はどっちかっていうと、「RPU-10特有」というよりは、一般的なモーション動作のプログラムはどうしたら作れるか?みたいな話です。コマンド式サーボの場合、目標角度と動作時間を指定することができますので、まずはお手軽に、ということで、プログラムよりこれらをサーボに指示し、あとは完了するのをのんびり待っているという方式で作ってみます。


■動作データをどのように持つか?
まずは動作データをプログラムから管理できるようにしないといけません。第一章で説明したように、ATmega128のFlash ROMは1万回の書き換えが可能ですから、外付けEEPROMとかは考えずに、Flash ROM上に配置することにします。つまり、動作データを書き換える時には、一緒にプログラムごと書き換えることになります。実は、いろいろやり方があるみたいで、avrdudeである部分だけ書き換えることも可能みたいなので、メモリ配置をちゃんとコントロールして、その場所へ書き込むようにすれば可能ではないかと思いますが、きっと誰かがやってくれるだろう、ということで、ここではそこまで突っ込まないことにします。


実際、SIPHA SYSTEMでは、上記のように、プログラムごと書き込んでいます。ただ、あまり開発環境を意識したくなかったので、SIHPA TERM上からポーズデータをソースファイル化して、その後、コンパイラやavrdudeを呼び出すようにしています(SIHPA TERMから「BUILD」というメニューを呼び出すと、makeが実行される)。また、ポーズ編集自身は、RAM上で行うようにしていまして、RAM上でポーズ編集、再生確認をした後、Flash ROMに転送するようにすることで、書込み回数をちょっと節約しています。


次に動作データの構造ですが、まずは素直に角度と時間を持つようにします。角度は10倍した値を扱いますので、-1500~1500、時間は最大いくつまで扱えるかわかりませんが、10msec単位で2バイト長のデータになっています。このあたりは、サーボの説明書に書いてありますので、それを見れば確認することができます。よって、構造体は以下のようにし、これを配列化します。


unsinged short usTime;
short sAngle;

あとは、何個、これが入っているか(またはデータの終わりを表す仕組み)が必要になりますが、今回は簡単に、「最後のsTimeが0だったらこのデータは終わり」という仕組みにしました。というわけで、こんな構造体を定義します(「struct」だけでいくか、「typedef」を使うかは趣味の問題です)。


typedef struct {
unsigned short usTime;
short sAngle;
} TPOSE;

■サンプルプログラム
なんか簡単に説明するのが難しいプログラムになっちゃってすいません。簡単に言うと、配列からデータを読み出しサーボに指示、メインループで一定時間待って次の指示を出す、というプログラムなのですが、常時、RS232Cからの指示を受け取るために、少々ややこしい構造になっています。本当はもう少しシンプルにもできるのですが、「何かをしながら」という処理をやるためには、こういった構造が必須になると思いますので、がんばって読んでください。


ポイントとしては、sStepという変数と、usTimeという変数です。sStepは、サーボ指示データ配列の読み出し場所を示しますが、動作しない場合は「-1」とすることで、動作中とアイドル中という意味も持っています。で、一発動作指示をだしたら動作時間をセットし、これを20msec毎に回るループ中で減算することで、サーボ動作完了タイミングにあわせています。


サーボが10msec単位で動作するにも関わらず、ループを20msecとした理由ですが、実際、20個以上のサーボに115200bpsで指示を出すと、結構時間がかかってしまいますので、今後のことをちょっと余裕を見てまずは20msecとしました。


   1: //—————————————————————————————-
2: // サーボをモーションっぽく動作
3: // Flash ROM上のデータを使ってモーションっぽくサーボを動作させます。
4: //
5: // 環境 RPU-10、GDL V2.00
6: // 説明 ビルドされた本プログラムをRPU-10へ転送後、パソコン側で「SIMPLE TERM」(GDLに
7: // 同梱)などを使って通信速度115200bpsで通信ポートを開いてください。その後RPU-10
8: // を再起動するとプログラムがスタートし、キーボード入力待ちになります。
9: //
10: // AUTHORED BY SISO JUNK STDUIO
11: //—————————————————————————————-
12: #include <avr/pgmspace.h>
13: #include <avr/io.h>
14: #include <avr/interrupt.h>
15: #include <avr/eeprom.h>
16: #include <stdio.h>
17: #include <avr/boot.h>
18: #include <avr/wdt.h>
19:
20: #include <sv.h>
21: #include <rs.h>
22:
23: #include <../ATmega128/rs0_printf_P.c> // URART0用フォーマット(ROM用)
24:
25:
26:
27: // 動作データ構造定義
28: // サーボに角度を与えるための情報を保持する構造体で、動作時間と目標角度で
29: // 構成しています。
30: typedef struct {
31: unsigned short usTime; // 動作時間(20ms単位)
32: short sAngle; // 目標角度(0.1度を10倍した値)
33: } TPOSE;
34:
35: // 動作データ設定
36: // Flash ROM領域にデータを設定します。
37: const TPOSE GPposeList[] PROGMEM = {
38: { 50, 300 }, // 30度まで1秒で動作
39: { 50, 0 }, // 0度まで1秒で動作
40: { 25, -300 }, // -30度まで0.5秒で動作
41: { 25, 0 }, // 0度まで0.5秒で動作
42: { 0, 0 }, // sTime=0としてデータの終了
43: };
44:
45:
46:
47: //
48: // メインルーチン
49: //
50: int main( void )
51: {
52: short sStep; // ステップ
53: unsigned short usTime; // 動作時間
54: unsigned char aucTBuff[256]; // RS485通信用バッファ
55: unsigned long ulTimer; // 0.833msecタイマ記憶用
56: TPOSE stPoseList; // ポーズリストデータ(RAM)
57:
58: RPU_InitConsole( br115200 ); // RPU-10ライブラリの初期化
59: SV_Init( br115200 ); // サーボ制御ライブラリの初期化
60: sei(); // 割り込み処理開始
61:
62: sStep = -1; // -1で停止と扱うので、事前初期化。
63:
64: // 起動メッセージの表示
65: rs0_puts_P( PSTR( “MOVE SERVO LIKE MOTION PLAY\n” ));
66:
67: // 1秒待つ(よく知らないけど必要らしい)
68: RPU_ResetTimerCounter();
69: while( RPU_GetTimerCounter10() < 100 );
70:
71: // 機能ガイダンスの表示
72: rs0_puts_P( PSTR( “PLEASE INPUT KEY a=START, b=STOP.\n” ));
73:
74: // メインループ
75: // 20msec毎にループします。
76: while( 1 ){
77: // 1ループが20msecになるように、あらかじめタイマー値を記憶します。
78: ulTimer = RPU_GetTimerCounter();
79:
80: // キー入力の受付
81: if( rs0_rx_buff() != 0 ){
82: switch( rs0_getc()){
83: case ‘a’: // 動作開始
84: // sStepを0にする(-1以外)ことで動作開始とします。また、sTimeを
85: // 0にすることにより、最初の指示を発行します。
86: sStep = 0;
87: usTime = 0;
88: // サーボ(1)トルクオン
89: SV_TorqueOnOff( aucTBuff, 1, 1 );
90: break;
91: case ‘b’: // ストップ
92: // サーボ(1)トルクオフ
93: SV_TorqueOnOff( aucTBuff, 1, 0 );
94: // ステップ動作も停止する。
95: sStep = -1;
96: rs0_puts_P( PSTR( “ABORT.\n” ));
97: break;
98: }
99: }
100:
101: // サーボ指示処理
102: // sStepが-1以外でかつ、sTimeが0のときに新しい角度指示を行います。
103: // 角度指示後、sTimeは20msec毎に減算されていき、0になったとき、
104: // また新しい角度指示を行います。
105: if( sStep != -1 ){
106: if( usTime == 0 ){
107: // Flash ROM領域からのポーズデータ読み込み
108: // 他にもやり方はありますが、とりあえずシンプルに…。
109: memcpy_P( &stPoseList, &GPposeList[sStep], sizeof( TPOSE ));
110:
111: if( stPoseList.usTime != 0 ){
112: // 角度指示
113: usTime = stPoseList.usTime;
114: SV_Angle( aucTBuff, 1, stPoseList.sAngle, usTime*2 );
115: // ステップ値の表示
116: rs0_printf_P( PSTR( “EXECUTING STEP %d\n” ), sStep );
117: // nStepをインクリメントし、次回指示にそなえます。
118: sStep++;
119: }
120: else{
121: // 再生終了処理
122: // 設定データの動作時間が0なので、動作を完了します。
123: rs0_puts_P( PSTR( “STOP.\n” ));
124: sStep = -1;
125: }
126: }
127: // 実行中監視タイマ値をデクリメントします。
128: if( usTime != 0 ) usTime–;
129: }
130:
131: // 20msec待ちます。
132: // 冒頭で保存した値と比較しますので、これでわりと正確に20msec待つ
133: // ことができます。RPU_GetTimerCounter()は、0.8333…msec単位の
134: // タイマカウンタです。そのため、20msecは24になります。
135: while( RPU_GetTimerCounter() < ( ulTimer + 24 ));
136: }
137:
138: return 1;
139: }

L78
この行でまずタイマー値の読み取りを行います。このループは20msecで一周するようにしたいので、ここでタイマー値を覚えておき、ループの最後で20msecに到達するように待つようにします。このタイマー値は、0.833…msecなので、24倍すれば20msecになります。単純に考えると、必ず20msecで一周するだけであれば、while()の外でタイマー値を読み取り、あとはループ内で20msec待つだけでいいのですが、この先プログラムが太ってきたときに、20msecを守れないケースもあるでしょう(例えば、シリアル通信を受けて、何か複雑な処理をするとか)。


そういうケースを考慮して、いつもこういう構造にしています。で、本番プログラムでは、処理タイムオーバーを検知する処理を入れておき、動作中に予想外に時間がかかっているところが無いかチェックするための指標としています。


L80~L98
キー入力処理です。例によって、受信を待たない処理としています。ここでは、基本的には先に説明したsStepという変数をいじって、サーボ指示処理の開始/終了を制御するだけです。


L100~L123
sStepが-1じゃない時、すなわち動作中に、必要に応じてサーボに指示を出します。「必要に応じて」ですが、これにはusTimeをチェックしています。この変数が0の場合は、動作完了しているときということになりますから、新しい指示をだすべきかどうかをチェックし、必要であればサーボに指示をし、そうでない場合は終了処理をします。終了かどうかは「動作設定データ」の時間が0になっていることをチェックして判断します。


GPposeListの内容を追加すれば、もっと長い動作を行うことも可能ですので、ぜひ試してみてください。


■今回使用した関数



  • unsigned long RPU_GetTimerCounter( void );
    0.83333333msec単位のタイマー値を読み出す関数です。12カウントで10msecになります。RPU_GetTimerCounter10()と同じような機能ですが、RPU_GetTimerCounter()の方をよく使っています。というのも、中身が見えないので、10msec単位のカウントといわれても、どういう10msecなのかわからないからです。見えないものを使う時は、こういうところで臆病になってしまいますね。




最後に、ちょっと御願いということで、もっと前に明確に書いておくべきだったなってことを追記しておきます。GDLにはRPU-10のライブラリが入っていますが、恐らくBest Technologyさんのご好意で、趣味人への1つのチャンスとして同梱してくださっていると思います。そんなわけで内容についてBest TechnologyさんやFUTABAさんに質問等をするのは無しにしておいてください。
あくまでも、無償で提供してくださっているツールにサポート無しという条件のおまけライブラリを使い、メーカーの想定外である再プログラミングという、とっても趣味で楽しい自己責任なことをやっているということで御願いします。
このあたりの注意書きについては、ネットにアップしている以上、検索エンジンであるエントリを見つけたりして、こういった背景を知らない方々にも見て頂くこともあるかなということで、過去のエントリも入れておきました。
もし、教育目的等で再プログラミングに関する調査をされている場合は、プログラミング学習目的でGR-001を販売している会社もあるようですので(「ヒューノイドロボット」で検索してみてください)、そちらにコンタクト願います。






※注意:本BLOGにてRPU-10での再プログラミングについての情報を公開していますが、これらはSISOが個人的に再プログラミングを行った時の技術情報を整理して紹介しています。GDLへのRPU-10ライブラリ同梱については、Best Technologyさんのご好意で、趣味人への1つのチャンスとして同梱してくださっていると理解しています。そのため、RPU-10の再プログラミングについては、くれぐれもご自身の責任で、また、Best TechnologyさんやFUTABAさんに問い合わせたりすることの無いようにお願いいたします。

  • このエントリーをはてなブックマークに追加
  • Pocket

関連前後記事

Comment

  1. stellar より:

    ちょっと、聞いても良いでしょうか?
    GDLでプログラムをした後、RPU-10へ転送すると、RPU-10ファームウェアが消えますが、
    以下のツールを使って、RPU-10ファームウェアの再書き込みを行ったことありますか?
    RPU11to10_HPI_1000.exe(784KB)
    http://www.hpirobot.jp/support/support.html#rpu-10-11
    RPU11to10_HPI_1000.exe を実行して得られるファイル内には、avrdude.exe , FLUSH_RPU10_ALL_20070724_1.hex などがあるので、GDLと同じ仕組みで、RPU-10ファームウェアは書き込まれると思われますので、できるような気もします。
    自分で試せば良いのですが、元に戻らなかったらと思うと、ためらってしまいます。
    SISOさんが経験済みか、情報をお持ちであれば教えて頂けないでしょうか?

  2. SiSO より:

    stellarさん>
    オリジナルプログラムへの書き換えはまだやったことがありません。というか、起動したことも無かったりして…仕組みから察するに問題なくできると思われます。今思うと、すごい暴走状態で作ってますねー。(^_^;
    きっと気になる人も多いかと思うので、誰からもコメントが無かったら、日曜日ぐらいにモーションエディタとリンクできるところぐらいまでは一度やってみます。

  3. urouro より:

    stellarさん>
     はじめまして、
    bootloaderを自作したプログラムで破壊していなければ、HPIさんから提供されている書き換えツールでオリジナルに戻せます。
    少し気をつけないといけないのは、自作プログラムを転送している場合には、書き換えツールを使用する時には、「RPUのプッシュボタンを押しながら電源を入れ」ないといけないと言うことです。
    オリジナルプログラム内部には、bootloaderに切り替える機能がありますが、自作プログラムにはオリジナルプログラムの書き換えツールに対応してbootloaderに切り替える機能が実装されていないためです。

  4. stellar より:

    urouro さん>
    はじめまして、
    RPUのプッシュボタンを押しながら電源を入れ、RPU-10ファームウェアへ書き換えれば良いのですね。貴重な情報ありがとうございます。これで、自作プログラムをトライできます。

  5. SiSO より:

    urouroさん>
    情報ありがとうございますー。普通にブートローダで書き込めばいいんですね!確か、前にいろいろ試したときに、RPU_Reboot()を使うと、自作プログラムからブートローダーのプログラム書き込み待ちになれたような。
    stellarさん>
    ちなみにプッシュボタンを押して起動すると、かなりの速度ですが、LEDが3回点滅します。ロータリースイッチの位置はどこでも大丈夫だと思いますよ。

  6. stellar より:

    urouro さん>
    SISO さん>
    RPU-11 -> 独自プログラム -> RPU-10 へ入れ替えできました。ありがとうございました。
    ロータリースイッチの番号は、どこでも良さそうですが、HPIのドキュメントに0番に合わせるように書かれているので、素直に 0番を使った方が良さそうです。

  7. SiSO より:

    stellarさんがオリジナルプログラムを書くと、どんなふうになるのか楽しみです。

  8. […] RPU-10講座/第九章 モーションっぽくサーボ動作 […]

Your Message

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

*

PAGE TOP ↑