RPU-10講座／第十一章 ３つのサーボ動作
すいません m(_ _)m、本記事はブログ引越時に書式が崩れました。順次修正中です。
当初予定では「４つのサーボ動作」でしたが、考えてみたら、１つのサブハブで３つまでしかサーボが接続できないので、３つにすることにしました。
さて、「第十章 RPU-10からサーボIDの書き換え」にて無事サーボIDが書き換わったということで、複数サーボの同時制御を説明します。サンプルプログラムは「第九章 モーションっぽくサーボ動作」をベースに、「プログラムによる角度補間処理」を加えたものです。…とさらっと書きましたが、かなりプログラムのボリュームはアップしていますので、がんばって解読してください。制御対象とするサーボIDは、31、32、33としています。
■どうやってサーボを複数制御するか？
まずは、複数サーボを制御するためのデータの管理方法について説明します。これに関してはいろいろな人がいろいろな方法で実装していると思いますが、基本的な考え方は同じようなものだと思います。
ちらちらと製品や他の人が開発したコントローラを見ていると、「ポーズ」という、各サーボの角度情報と実行時間で管理するものが多いのではないかと思います。実行時間については、このポーズに到達する時間のものあれば、このポーズから次のポーズへ移る時間など、扱い方はそれぞれだと思いますが、今回は、ポーズへ到達するための時間として扱います。と、改まって説明なんか書いていたりしますが、実は既に「第九章 モーションっぽくサーボ動作」にて、「時間と角度」という形でデータを扱っています。今回は、これを基本として複数のサーボに対応させる、つまり角度情報を配列化するだけでよいかと思います。
RS301の場合、0.1度単位での指定が可能で、かつ、範囲は「-150.0～150.0度」です。そのため、このデータを保持するためには、short型の変数である必要があります。ということは、例えば24サーボのデータを保持しようとした場合、48バイト＋２バイト（動作時間）＝50バイトというデータが必要になります。これを「１ポーズ」と呼ぶとして、ATmega128のFlash ROMは128KByteありますが、このうち64KByteを動作データ保持に割り当てれたとしても、1310ポーズになります。実際には、その他の管理データも必要でしょうから、もっと容量は大きくなると思います。
これが多いか少ないかはそれぞれの感覚があると思いますが、１モーションを20ポーズで固定した場合、65モーションになります。これまた多いか少ないかはそれぞれの感覚になってしまいますが、SISOの場合、少ないなと思って、あれこれデータの載せ方を工夫し、もっと入るようにしています。また以前のSIPHA SYSTEMでは、「前ポーズとの差分のみ」を次のポーズデータとして持つようにすることで、容量を小さくしていました。
こんな感じでデータの保持方法を設計しています。
■サーボ動作角度補間処理
今回は、RS301に「時間」と「角度」を指示するのではなく、20msec毎に到達すべき角度をRS301に指示するというやり方にしました。なんだかせっかくのコマンド式サーボなのにもったいない感じですが、後々でジャイロ処理を入れようと思うと、今のところこの方法しか思いつきません。
例えば、「１秒で０度から５０度まで動作する」という指示を出したい場合、20msec毎に「20msecで１度」、「20msecで２度」…と指示をしていきます。20msec単位ではありますが、これで滑らかにサーボを動作させようというものです。
直線補間の場合、計算は簡単です。
指示角度＝初期角度＋（目標角度－初期角度）＊（経過時間／目標時間）
となります。「（目標角度－初期角度）」にて目標時間で動作させるべき角度を求めます。これに（経過時間／目標時間）で現在の進行割合を求め、これらを掛け合わせることで、最初の角度からどれだけ動かせばいいのかがわかります。この結果に初期角度を足せば、その時々の指示角度を求めることができます。
ただし、この計算は整数計算で行いますので、桁落ちを考慮しなくてはいけません。例えば、short型で（１／５０）を実行すると、答えは「０」になってしまいます。なぜならば、short型は小数点以下を扱うことができないからです。浮動小数点型を使えばこの問題は解決できますが、ここは速度を稼ぐために整数型を使用します。そこで、先の計算式は以下のようにするのが良いでしょう。
指示角度＝初期角度＋（目標角度－初期角度）＊経過時間／目標時間
場合によっては、今度は大きい方で桁あふれすることが考えられますので、さらにlong型にcastしながら計算を行います。
■サンプルプログラム
「第九章 モーションっぽくサーボ動作」で紹介したサンプルプログラムをベースに、複数サーボ制御、サーボ動作角度補間処理を入れたため、結構なボリュームになってしまいました。
とはいっても、なるべく基本構造は同じになるように考慮しています。また今回のプログラムは、起動するとすぐにサーボトルクをオンにしますので、実行するときは注意してください。
プログラム内容ですが、キー入力を受け付けて処理開始を登録し、処理が必要だったら新しいサーボ指示値を登録する、というところまでは同じ構造ですが、「第九章 モーションっぽくサーボ動作」のサンプルプログラムと異なるのは、サーボに直接指示を出すのではなく、一旦、変数に登録しているところです。
この変数は、主に新たに追加された関数、moveServo()から参照されます。moveServo()が今回のポイントで、新たに指示値が登録されたら、データを初期化し、まずは最初の指示をサーボに出します。以降、呼び出されるたびに自分でカウントアップをしながら計算してサーボに指示値を送り、指示処理が完了したら動作要求時間などを０クリアします。
呼び出し側であるmain()から見ると、とにかく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: //
11: // AUTHORED BY SISO JUNK STDUIO
12: //—————————————————————————————-
13: #include <avr/pgmspace.h>
14: #include <avr/io.h>
15: #include <avr/interrupt.h>
16: #include <avr/eeprom.h>
17: #include <stdio.h>
18: #include <avr/boot.h>
19: #include <avr/wdt.h>
20:
21: #include <sv.h>
22: #include <rs.h>
23:
24: #include <../ATmega128/rs0_printf_P.c> // URART0用フォーマット（ROM用）
25:
26:
27:
28: // 定数定義
29: #define SERVO_MAX 3 // 制御対象サーボ数
30:
31: // 通信バッファ
32: unsigned char GRaucTBuff[256]; // RS485通信用バッファ
33:
34: // 動作データ構造定義
35: // サーボに角度を与えるための情報を保持する構造体で、動作時間と目標角度で
36: // 構成しています。
37: typedef struct {
38: unsigned short usTime; // 動作時間（20ms単位）
39: short asAngle[SERVO_MAX]; // 目標角度（0.1度を10倍した値）
40: } TPOSE;
41:
42: // 動作データ設定
43: // Flash ROM領域にデータを設定します。
44: const TPOSE GPposeList[] PROGMEM = {
45: // TIME SERV31 SERVO32 SERVO33
46: { 50, { 300, 300, 300 }},
47: { 50, { 0, 0, -300 }},
48: { 50, { 0, -300, -300 }},
49: { 50, { -300, -300, -300 }},
50: { 25, { 0, 0, 0 }},
51: { 0, { 0, 0, 0 }},
52: };
53:
54: // サーボ動作管理
55: typedef struct {
56: unsigned short usReqTime; // 動作要求時間（20ms単位）
57: unsigned short usCurTime; // 制御時間（20ms単位）
58: short asReqAngle[SERVO_MAX]; // 要求角度
59: short asCurAngle[SERVO_MAX]; // 現在指示角度
60: short asBgnAngle[SERVO_MAX]; // 処理開始時角度
61: } TSRVM;
62: // サーボ動作管理データの設定(RAM)
63: TSRVM GRservoMgr;
64:
65:
66: //
67: // サーボ動作管理
68: // 要求された時間、角度により、分割した角度指示をサーボに行います。
69: //
70: void moveServo( void ){
71: unsigned char ucID;
72:
73: // 新規にデータ登録されたかどうかをチェックする。
74: if(( GRservoMgr.usReqTime != 0 )&&( GRservoMgr.usCurTime == 0 )){
75: // 新規にデータ登録された場合、各データを初期化する。
76: for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
77: GRservoMgr.asBgnAngle[ucID] = GRservoMgr.asCurAngle[ucID];
78: GRservoMgr.usCurTime = 0;
79: }
80: // 制御時間をインクリメントします。
81: GRservoMgr.usCurTime++;
82: }
83:
84: // サーボ角度補間処理
85: if(( GRservoMgr.usReqTime != 0 )&&( GRservoMgr.usCurTime != 0 )){
86: for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
87: // 補間計算
88: // 計算の順番に注意してください。また、桁あふれしないようにlong型に
89: // castして計算しています。
90: GRservoMgr.asCurAngle[ucID] = GRservoMgr.asBgnAngle[ucID] +
91: (short)((long)( GRservoMgr.asReqAngle[ucID] – GRservoMgr.asBgnAngle[ucID] ) *
92: (long)GRservoMgr.usCurTime / (long)GRservoMgr.usReqTime );
93: }
94: // 制御時間をインクリメントします。
95: GRservoMgr.usCurTime++;
96: if( GRservoMgr.usReqTime < GRservoMgr.usCurTime ){
97: // 終了処理
98: GRservoMgr.usReqTime = 0;
99: GRservoMgr.usCurTime = 0;
100: }
101: }
102:
103: // サーボへの角度指示
104: // 計算結果（計算していない場合は前のままの値）をサーボに指示します。
105: for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
106: SV_Angle( GRaucTBuff, ucID+31, GRservoMgr.asCurAngle[ucID], 2 );
107: }
108: }
109:
110:
111: //
112: // メインルーチン
113: //
114: int main( void ){
115: short sStep; // ステップ
116: unsigned char ucID; // サーボID
117: unsigned long ulTimer; // 0.833msecタイマ記憶用
118:
119: RPU_InitConsole( br115200 ); // RPU-10ライブラリの初期化
120: SV_Init( br115200 ); // サーボ制御ライブラリの初期化
121: sei(); // 割り込み処理開始
122:
123: sStep = -1; // -1で停止と扱うので、事前初期化。
124: // どかっとサーボ管理データ初期化
125: memset( &GRservoMgr, 0x00, sizeof( TSRVM ));
126:
127: // １秒待つ（よく知らないけど必要らしい）
128: RPU_ResetTimerCounter();
129: while( RPU_GetTimerCounter10() < 100 );
130:
131: // 起動メッセージの表示
132: rs0_puts_P( PSTR( “MOVE 3 SERVOS LIKE MOTION PLAY\n” ));
133: rs0_puts_P( PSTR( “PLEASE INPUT KEY a=START, b=STOP.\n” ));
134:
135: // サーボ(31,32,33)トルクオン
136: for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
137: SV_TorqueOnOff( GRaucTBuff, ucID+31, 1 );
138: }
139:
140: // メインループ
141: // 20msec毎にループします。
142: while( 1 ){
143: // １ループが20msecになるように、あらかじめタイマー値を記憶します。
144: ulTimer = RPU_GetTimerCounter();
145:
146: // キー入力の受付
147: if( rs0_rx_buff() != 0 ){
148: switch( rs0_getc()){
149: case ‘a’: // 動作開始
150: // sStepを０にする（-1以外）ことで動作開始とします。
151: sStep = 0;
152: rs0_puts_P( PSTR( “START\n” ));
153: break;
154: case ‘b’: // ストップ
155: // ステップ動作を停止する。
156: sStep = -1;
157: // サーボ制御処理も新たな計算を停止させる。
158: GRservoMgr.usReqTime = 0;
159: GRservoMgr.usCurTime = 0;
160: rs0_puts_P( PSTR( “STOP\n” ));
161: break;
162: }
163: }
164:
165: // サーボ指示処理
166: // sStepが-1以外でかつ、sTimeが０のときに新しい角度指示を行います。
167: // 角度指示後、sTimeは20msec毎に減算されていき、０になったとき、
168: // また新しい角度指示を行います。
169: if( sStep != -1 ){
170: if( GRservoMgr.usReqTime == 0 ){
171: if( pgm_read_word( &GPposeList[sStep].usTime ) != 0 ){
172: // 指示情報表示
173: rs0_printf_P( PSTR( “STEP=%d ” ), sStep );
174: // 角度指示
175: GRservoMgr.usReqTime = pgm_read_word( &GPposeList[sStep].usTime );
176: rs0_printf_P( PSTR( “ReqTime=%d ” ), GRservoMgr.usReqTime );
177: for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
178: GRservoMgr.asReqAngle[ucID] = pgm_read_word( &GPposeList[sStep].asAngle[ucID] );
179: rs0_printf_P( PSTR( “ReqAngle[%d]=%d ” ), ucID+31, GRservoMgr.asReqAngle[ucID] );
180: }
181: rs0_printf_P( PSTR( “\n” ));
182: // nStepをインクリメントし、次回指示にそなえます。
183: sStep++;
184: }
185: else{
186: // 再生終了処理
187: // 設定データの動作時間が０なので、動作を完了します。
188: sStep = -1;
189: }
190: }
191: }
192:
193: // サーボ動作管理と指示
194: // この処理は常時20msec毎に呼び出され、角度指示を行います。
195: moveServo();
196:
197: // 20msec待ちます。
198: // 冒頭で保存した値と比較しますので、これでわりと正確に20msec待つ
199: // ことができます。RPU_GetTimerCounter()は、0.8333…msec単位の
200: // タイマカウンタです。そのため、20msecは24になります。
201: while( RPU_GetTimerCounter() < ( ulTimer + 24 ));
202: }
203:
204: return 1;
205: }
さてさて、内容の解説ですが、main関数の内容はほとんど「第九章 モーションっぽくサーボ動作」と同じになるよう、あれこれ考慮して作成してみました。大きく異なっているところは、「moveServo」という関数にまとめました。
moveServo()という関数は、RAM上のデータを見て自分で何をするべきかを判定し、呼び出し側（main）からはひたすら20msec毎に呼び出していれば、勝手に動作するようにプログラムしています。これは、SISO好みのやり方でして、スレッド風プログラミングとでも言えばいいんでしょうか、こういう構造でいくつかの関数を作っておき、メインループから呼び出すようにしています。関連するデータをmain()関数の方でごちゃごちゃいじっていますが、実際にSIPHA SYSTEMに実装するときは、もうちょっと関数でアクセスするようにして、なるべく関連するデータ構造を意識しなくてもいいように工夫しています。
細かに説明すると大変ですので、がんばって解析してみてください。ヒントを以下に書きます。
- 新規データ登録の判定
メイン関数から新たに動作要求が発生すると、動作要求時間が０から要求された時間に変わります。通常、moveServo()が何もしていない時は、制御時間も０ですので、両方のデータが０だった場合に新しいデータが登録された、と判定するようになっています。 - サーボ角度補完計算の要否
逆に、両方とも０じゃない場合は、サーボ角度補完計算をする必要があります。この時、最初の計算を制御時間＝０で計算すると、「０度から３０度まで」という指示があった場合、最初の指示が「０度」になってしまいます。そのため、制御時間は新規データ登録検出時にあらかじめ「１」に加算しています。
あと、L171にて「pgm_read_word( &GPposeList[sStep].usTime )」という関数がいきなり登場しますが、
これは、ATmega128（他のAVRも同じかもしれませんが）独特の、「Flash ROM領域からのデータ
読み出し」になります。他にも何種類かありますが、２バイト長のデータを読み出すときに使用します。この行では「GPposeList[sStep].usTime」の内容を読み出しています。
■動作結果
こんな感じで動作します。
追記：すいません、サーボ並べる順番、間違えました…。
■おまけで調査
実際にデータがちゃんと送信されているかどうか見えないので、RSC-U485を使って簡単なパケットモニタをつくり、RS485上でどのようなデータが流れているか調べてみました。アイドルのとき（ひたすら０度を出力している状態）には、以下のデータがひたすら流れていることを確認できました。プログラムは思惑通りに動いているようです。内容については、「RS301CR/RS302CD取扱説明書」を参照してください。
FAAF1F001E04010000020006
FAAF20001E04010000020039
FAAF21001E04010000020038
※注意：本BLOGにてRPU-10での再プログラミングについての情報を公開していますが、これらはSISOが個人的に再プログラミングを行った時の技術情報を整理して紹介しています。GDLへのRPU-10ライブラリ同梱については、 Best Technologyさんのご好意で、趣味人への１つのチャンスとして同梱してくださっていると理解しています。そのため、RPU-10の再プログラミングについては、くれぐれもご自身の責任で、また、Best TechnologyさんやFUTABAさんに問い合わせたりすることの無いようにお願いいたします。
SiSO-LAB所長兼研究員のSiSOです。あ、趣味のサイトなので実体はありませんけど…。
[…] RPU-10講座／第十一章 ３つのサーボ動作 […]