前回でサーボの角度補間制御、ジャイロセンサの取り込みまでできましたので、今回はいよいよサーボへジャイロセンサのフィードバックを行います。一番初歩的ですが、コントローラとPWM式サーボの間にジャイロセンサを入れたような動きを実現してみます。

■サーボ動作への反映方法
さて、ここからが実は問題です。RS301はコマンド式サーボなので、目標角度と動作時間を指示します。ところが、「サーボでフィードバック制御」というと、例えば直立でジーっとしている時にユラユラっと制御したりする必要があります。というわけで、なんだか動作時間が設定できるのにもったいない感じですが、20msec単位で常時角度を指示する方式を使う必要があります（他にいいやり方があったら教えてください）。

「RPU-10講座／第十一章 ３つのサーボ動作」では、角度と時間をサーボに与える方式だと解説がさみしかたったので、ついつい勢いで20msec単位で常時角度を指示する方式を実装してしまいましたが、実は、ジャイロフィードバックをかけたり、他のセンサやコントローラパッドからの指示でリアルタイムに操作するための構造です。

■ジャイロフィードバック
ジャイロフィードバック制御を行うためにには、ジャイロの制御値をサーボ指示角度に反映する必要があります。前章で、KRG-3の場合は中心値が「260～270あたり」、ぶんぶん振り回すと「±100」程度で値が変わることがわかっていますので、中心値からの差分に適切な係数を掛け、それをサーボ指示角度から加減算すればよいことになります。

補正角度＝サーボ指示角度＋（ジャイロ入力値－中心値）／ジャイロ変化最大値＊係数

一番シンプルにはこんな感じですが、実際には、補正角度を求めるためにはもう少しいろいろなこと（値の平均化とか）をした方が、よい結果を得られることが多いようです。そこはみなさんで研究してみてください。実際にソースコードの方では、桁落ちを考慮して演算の順序を変更し、桁あふれを考慮してlong型にcastしています。

■サンプルプログラム
今回のサンプルプログラムは数行、「RPU-10講座／第十一章 ３つのサーボ動作」で使用したサンプルプログラムに追加しただけです（「数行」といいながら、数えてみたら１７行でした…）。

追加箇所は以下のとおりです。

• L32～L36
  ジャイロから得た値から、フィードバックする角度に計算するための定義を追加しました。実際に実機に載せているプログラムでは、ジャイロフィードバックのON/OFFやレベルの変更を動的に行いたいので、変数を使ってレベル変更可能なようにプログラミングしていますが、ここではシンプルにサンプル、ということで、固定定義にて行っています。


• L81
  RPU-10ライブラリのジャイロ読み取り関数からの値を受けるための変数を追加しました。


• L82
  各サーボ用のジャイロフィードバック値を保持するための変数を追加しました。


• L114～L122
  ジャイロフィードバックの計算処理を追加しました。本サンプルプログラムでは、一発で計算していますが、前回値と平均を取ったり、なまらせたりと、いろいろなやり方があると思います。


• L127
  ジャイロフィードバック値をサーボ指示角度に反映するように変更しました。ここの演算のさせ方にはいろいろな考え方があると思います。今回の例では、オリジナルの指示値に対して一時的に反映する、という形を取っていますが、例えば、次の補間計算時のスタート角度として、ジャイロでフィードバックされた角度も含めるといったやり方もあるかと思います。ここに解答はありませんので、みなさんでいろいろ研究してみてください。

   1: //—————————————————————————————-

2: //  ジャイロフィードバック

3: //      Flash ROM上のデータを使ってモーションっぽく３つのサーボを動作させている間に、

4: //      ジャイロフィードバックをするという、まんまコアなサンプルプログラムです。

5: //

6: //  環境    RPU-10、GDL V2.00

7: //  説明    ビルドされた本プログラムをRPU-10へ転送後、パソコン側で「SIMPLE TERM」（GDLに

8: //          同梱）などを使って通信速度115200bpsで通信ポートを開いてください。その後RPU-10

9: //          を再起動するとプログラムがスタートし、キーボード入力待ちになります。また、

10: //          プログラム起動後、すぐにサーボトルクがオンになりますので注意してください。

11: //

12: //      AUTHORED BY SISO JUNK STDUIO

13: //—————————————————————————————-

14: #include    <avr/pgmspace.h>

15: #include    <avr/io.h>

16: #include    <avr/interrupt.h>

17: #include    <avr/eeprom.h>

18: #include    <stdio.h>

19: #include    <avr/boot.h>

20: #include    <avr/wdt.h>

21:

22: #include    <sv.h>

23: #include    <rs.h>

24:

25: #include    <../ATmega128/rs0_printf_P.c>   //  URART0用フォーマット（ROM用）

26:

27:

28:

29: //  定数定義

30: #define     SERVO_MAX           3           //  制御対象サーボ数

31:

  32: //  ジャイロの設定値について

33: //      それぞれの環境で異なると思いますので、適切な値を設定してください。

34: #define     GYRO_ORIGIN         270L        //  ジャイロ取り込み中心値

35: #define     GYRO_WIDTH          110L        //  ジャイロ振れ幅値

36: #define     GYRO_LEVEL          100L        //  ジャイロ振れ幅に対して角度に反映する量

  37:

38:

39: //  通信バッファ

40: unsigned char   GRaucTBuff[256];            //  RS485通信用バッファ

41:

42: //  動作データ構造定義

43: //      サーボに角度を与えるための情報を保持する構造体で、動作時間と目標角度で

44: //      構成しています。

45: typedef struct {

46:     unsigned short  usTime;                 //  動作時間（20ms単位）

47:     short           asAngle[SERVO_MAX];     //  目標角度（0.1度を10倍した値）

48: } TPOSE;

49:

50: //  動作データ設定

51: //      Flash ROM領域にデータを設定します。

52: const TPOSE GPposeList[] PROGMEM = {

53:     //  TIME    SERV31  SERVO32 SERVO33

54:     {   50,     {   300,    300,    300     }},

55:     {   50,     {   0,      0,      -300    }},

56:     {   50,     {   0,      -300,   -300    }},

57:     {   50,     {   -300,   -300,   -300    }},

58:     {   25,     {   0,      0,      0       }},

59:     {   0,      {   0,      0,      0       }},

60: };

61:

62: //  サーボ動作管理

63: typedef struct  {

64:     unsigned short  usReqTime;              //  動作要求時間（20ms単位）

65:     unsigned short  usCurTime;              //  制御時間（20ms単位）

66:     short           asReqAngle[SERVO_MAX];  //  要求角度

67:     short           asCurAngle[SERVO_MAX];  //  現在指示角度

68:     short           asBgnAngle[SERVO_MAX];  //  処理開始時角度

69: } TSRVM;

70: //  サーボ動作管理データの設定(RAM)

71: TSRVM   GRservoMgr;

72:

73:

74:

75: //

76: //  サーボ動作管理

77: //      要求された時間、角度により、分割した角度指示をサーボに行います。

78: //

79: void    moveServo( void ){

80:     unsigned char   ucID;

  81:     unsigned short  usGyroX, usGyroY;       //  ジャイロセンサ用変数

82:     short           asGyroFb[SERVO_MAX];    //  ジャイロフィードバック計算結果

83:

84:     //  新規にデータ登録されたかどうかをチェックする。

85:     if(( GRservoMgr.usReqTime != 0 )&&( GRservoMgr.usCurTime == 0 )){

86:         //  新規にデータ登録された場合、各データを初期化する。

87:         for( ucID = 0; ucID < SERVO_MAX; ucID++ ){

88:             GRservoMgr.asBgnAngle[ucID] = GRservoMgr.asCurAngle[ucID];

89:             GRservoMgr.usCurTime = 0;

90:         }

91:         //  制御時間をインクリメントします。

92:         GRservoMgr.usCurTime++;

93:     }

94:

95:     //  サーボ角度補間処理

96:     if(( GRservoMgr.usReqTime != 0 )&&( GRservoMgr.usCurTime != 0 )){

97:         for( ucID = 0; ucID < SERVO_MAX; ucID++ ){

98:             //  補間計算

99:             //      計算の順番に注意してください。また、桁あふれしないようにlong型に

100:             //      castして計算しています。

101:             GRservoMgr.asCurAngle[ucID] = GRservoMgr.asBgnAngle[ucID] +

102:                     (short)((long)( GRservoMgr.asReqAngle[ucID] – GRservoMgr.asBgnAngle[ucID] ) *

103:                     (long)GRservoMgr.usCurTime / (long)GRservoMgr.usReqTime );

104:         }

105:         //  制御時間をインクリメントします。

106:         GRservoMgr.usCurTime++;

107:         if( GRservoMgr.usReqTime < GRservoMgr.usCurTime ){

108:             //  終了処理

109:             GRservoMgr.usReqTime = 0;

110:             GRservoMgr.usCurTime = 0;

111:         }

112:     }

113:

 114:     //  ジャイロフィードバック値の計算

115:     //      ジャイロフィードバック量を取り込み、値を角度補正値に変換します。例によって

116:     //      桁落ちしないように計算順序を変更し、桁あふれしないようにlong型にしています。

117:     //      サーボ31にX軸ジャイロ、サーボ32にY軸ジャイロ、サーボ33はフィードバック無し

118:     //      としています。

119:     RPU_GetGyro( &usGyroX, &usGyroY );

120:     asGyroFb[0] = ((long)usGyroX – GYRO_ORIGIN ) * GYRO_LEVEL / GYRO_WIDTH;

121:     asGyroFb[1] = ((long)usGyroY – GYRO_ORIGIN ) * GYRO_LEVEL / GYRO_WIDTH;

122:     asGyroFb[2] = 0;

123:

124:     //  サーボへの角度指示

125:     //      計算結果（計算していない場合は前のままの値）をサーボに指示します。

126:     for( ucID = 0; ucID < SERVO_MAX; ucID++ ){

 127:         SV_Angle( GRaucTBuff, ucID+31, GRservoMgr.asCurAngle[ucID] + asGyroFb[ucID], 2 );

128:     }

129: }

130:

131:

132: //

133: //  メインルーチン

134: //

135: int main( void ){

136:     short           sStep;                  //  ステップ

137:     unsigned char   ucID;                   //  サーボID

138:     unsigned long   ulTimer;                //  0.833msecタイマ記憶用

139:

140:     RPU_InitConsole( br115200 );            //  RPU-10ライブラリの初期化

141:     SV_Init( br115200 );                    //  サーボ制御ライブラリの初期化

142:     sei();                                  //  割り込み処理開始

143:

144:     sStep = -1;                             //  -1で停止と扱うので、事前初期化。

145:                                             //  どかっとサーボ管理データ初期化

146:     memset( &GRservoMgr, 0x00, sizeof( TSRVM ));

147:

148:     //  １秒待つ（よく知らないけど必要らしい）

149:     RPU_ResetTimerCounter();

150:     while( RPU_GetTimerCounter10() < 100 );

151:

152:     //  起動メッセージの表示

153:     rs0_puts_P( PSTR( “MOVE 3 SERVOS LIKE MOTION PLAY\n” ));

154:     rs0_puts_P( PSTR( “PLEASE INPUT KEY a=START, b=STOP.\n” ));

155:

156:     //  サーボ(31,32,33)トルクオン

157:     for( ucID = 0; ucID < SERVO_MAX; ucID++ ){

158:         SV_TorqueOnOff( GRaucTBuff, ucID+31, 1 );

159:     }

160:

161:     //  メインループ

162:     //      20msec毎にループします。

163:     while( 1 ){

164:         //  １ループが20msecになるように、あらかじめタイマー値を記憶します。

165:         ulTimer = RPU_GetTimerCounter();

166:

167:         //  キー入力の受付

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

169:             switch( rs0_getc()){

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

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

172:                 sStep = 0;

173:                 rs0_puts_P( PSTR( “START\n” ));

174:                 break;

175:             case ‘b’:                           //  ストップ

176:                 //  ステップ動作を停止する。

177:                 sStep = -1;

178:                 //  サーボ制御処理も新たな計算を停止させる。

179:                 GRservoMgr.usReqTime = 0;

180:                 GRservoMgr.usCurTime = 0;

181:                 rs0_puts_P( PSTR( “STOP\n” ));

182:                 break;

183:             }

184:         }

185:

186:         //  サーボ指示処理

187:         //      sStepが-1以外でかつ、sTimeが０のときに新しい角度指示を行います。

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

189:         //      また新しい角度指示を行います。

190:         if( sStep != -1 ){

191:             if( GRservoMgr.usReqTime == 0 ){

192:                 if( pgm_read_word( &GPposeList[sStep].usTime ) != 0 ){

193:                     //  指示情報表示

194:                     rs0_printf_P( PSTR( “STEP=%d ” ), sStep );

195:                     //  角度指示

196:                     GRservoMgr.usReqTime = pgm_read_word( &GPposeList[sStep].usTime );

197:                     rs0_printf_P( PSTR( “ReqTime=%d ” ), GRservoMgr.usReqTime );

198:                     for( ucID = 0; ucID < SERVO_MAX; ucID++ ){

199:                         GRservoMgr.asReqAngle[ucID] = pgm_read_word( &GPposeList[sStep].asAngle[ucID] );

200:                         rs0_printf_P( PSTR( “ReqAngle[%d]=%d ” ), ucID+31, GRservoMgr.asReqAngle[ucID] );

201:                     }

202:                     rs0_printf_P( PSTR( “\n” ));

203:                     //  nStepをインクリメントし、次回指示にそなえます。

204:                     sStep++;

205:                 }

206:                 else{

207:                     //  再生終了処理

208:                     //      設定データの動作時間が０なので、動作を完了します。

209:                     sStep = -1;

210:                 }

211:             }

212:         }

213:

214:         //  サーボ動作管理と指示

215:         //      この処理は常時20msec毎に呼び出され、角度指示を行います。

216:         moveServo();

217:

218:         //  20msec待ちます。

219:         //      冒頭で保存した値と比較しますので、これでわりと正確に20msec待つ

220:         //      ことができます。RPU_GetTimerCounter()は、0.8333…msec単位の

221:         //      タイマカウンタです。そのため、20msecは24になります。

222:         while( RPU_GetTimerCounter() < ( ulTimer + 24 ));

223:     }

224:

225:   return 1;

226: }