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

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

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

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

unsinged short usTime;

short          sAngle;

あとは、何個、これが入っているか（またはデータの終わりを表す仕組み）が必要になりますが、今回は簡単に、「最後のsTimeが０だったらこのデータは終わり」という仕組みにしました。というわけで、こんな構造体を定義します（「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:     //  １秒待つ（よく知らないけど必要らしい）

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:         //  １ループが20msecになるように、あらかじめタイマー値を記憶します。

78:         ulTimer = RPU_GetTimerCounter();

79:

80:         //  キー入力の受付

81:         if( rs0_rx_buff() != 0 ){

82:             switch( rs0_getc()){

83:             case ‘a’:                           //  動作開始

84:                 //  sStepを０にする（-1以外）ことで動作開始とします。また、sTimeを

85:                 //  ０にすることにより、最初の指示を発行します。

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が０のときに新しい角度指示を行います。

103:         //      角度指示後、sTimeは20msec毎に減算されていき、０になったとき、

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:                     //      設定データの動作時間が０なので、動作を完了します。

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をチェックしています。この変数が０の場合は、動作完了しているときということになりますから、新しい指示をだすべきかどうかをチェックし、必要であればサーボに指示をし、そうでない場合は終了処理をします。終了かどうかは「動作設定データ」の時間が０になっていることをチェックして判断します。

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

■今回使用した関数

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

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