RPU-10講座／第十六章 ロングパケットでサーボ動作
すいません m(_ _)m、本記事はブログ引越時に書式が崩れました。順次修正中です。
今回は、より実践的な通信方法として、ロングパケットによる複数サーボへの指示方法を解説します。
■ロングパケット
RS301へコマンドを送る方法として、ショートパケットとロングパケットという方法があります。違いは、前者が１つのサーボを対象としてデータを送信するのに対して（例外として、接続されているすべてのサーボというのがありますが）、後者は、複数のサーボにデータを送信します。複数のサーボへ送信する場合、サーボID(VID)とデータで構成された情報を必要数分だけ１パケットに設定することで行います。このとき、ヘッダ直後のID（３バイト目）は00Hにする必要があります。
利点としては、複数のサーボにデータを送信する場合に、送信するバイト数を減らすことができます。例えば、前回の「RPU-10講座／第十一章 ３つのサーボ動作」で紹介したサンプルプログラムの場合、３サーボ分で、36バイトになりますが、これをロングパケットで指定した場合、23バイトになります。３サーボぐらいですとあまり差はありませんが、これが２４個になると、288バイトと128バイトということで、かなりの差になります。
また、ロングパケットで送信した場合に、サーボの動作開始時間のずれがどれくらいあるのかはわかりませんが、少なくともデータが少ない分だけ、ショートパケットで指示をするよりは時差が少なそうです。
（2008/04/30追記：頂いた情報によると、ロングパケットの場合、チェックサムを受信したタイミングで動作を開始するようです。こっちでもまたFUTABAの方に確認しておきます）
■SV_TxPacket()を使って送信
ロングパケットを送信する場合、チェックサムなどの計算はロングパケットもショートパケットも同じですし、IDについても0を設定するだけです。あとはデータ部分をロングパケットとして作ってやれば、SV_TxPacket()でデータを送信できることになります。
SV_TxPacket()を使う難点としては、「バッファ用変数を用意して、そこでデータを組み上げてから渡す必要がある」ということで、メモリ使用量の点で少々難があることです。このあたりは、ロングパケット専用の送信関数を自前で用意した方がよさそうです。もう少し解析が進んだら、このあたりも着手しようと思っていますが…どなたかやってくださっても歓迎です。
データの組み上げには、構造体を使用するのが便利ですが、RPU-10ライブラリのsv.hを見ると、なぜかそれようと思われる「SRV_PACKET」という構造体が定義されているので、これをそのまま使用します（「time」は「unsigned short」で定義する方が正しいと思いますが…）。また、指示するときは、角度、時間、ともに送信するようにしてください。動作すると指示時間がクリアされますので。（ショートパットとの時はクリアされないような気がするんですが…）
■サンプルプログラム
「RPU-10講座／第十一章 ３つのサーボ動作」をベースに、サーボに角度指示データを送信する部分を変更しています。「RPU-10講座／第十二章 ジャイロセンサの接続」をベースにした方が、より実践的だとは思ったのですが、少しでもプログラムが短いほうが理解してもらいやすいかと思い、単純なサーボ動作の方にしました。また、ロングパケット化して送信する部分ですが、moveServo()中にベタベタっと書くのいうもありでしたが、前プログラムとの違いがわかりやすいように、新たに「sendAngles()」という関数を用意しました。一応、例によって手抜きのPacket Monitorでパケットは確認してみました。良好良好。
以下、サンプルプログラムです。なるべく「RPU-10講座／第十一章 ３つのサーボ動作」のサンプルプログラムと比較しやすいように書いてみました。
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 sendAngles( void ){
71: unsigned char ucID;
72: SRV_PACKET asvrPacket[SERVO_MAX];
73:
74: for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
75: asvrPacket[ucID].id = ucID + 31;
76: asvrPacket[ucID].angle = GRservoMgr.asCurAngle[ucID];
77: asvrPacket[ucID].time = 2;
78: }
79:
80: SV_TxPacket( GRaucTBuff, 0, 0x00, 0x1E, 5, 3, (unsigned char*)asvrPacket, 15 );
81: }
82:
83:
84: //
85: // サーボ動作管理
86: // 要求された時間、角度により、分割した角度指示をサーボに行います。
87: //
88: void moveServo( void ){
89: unsigned char ucID;
90:
91: // 新規にデータ登録されたかどうかをチェックする。
92: if(( GRservoMgr.usReqTime != 0 )&&( GRservoMgr.usCurTime == 0 )){
93: // 新規にデータ登録された場合、各データを初期化する。
94: for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
95: GRservoMgr.asBgnAngle[ucID] = GRservoMgr.asCurAngle[ucID];
96: GRservoMgr.usCurTime = 0;
97: }
98: // 制御時間をインクリメントします。
99: GRservoMgr.usCurTime++;
100: }
101:
102: // サーボ角度補間処理
103: if(( GRservoMgr.usReqTime != 0 )&&( GRservoMgr.usCurTime != 0 )){
104: for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
105: // 補間計算
106: // 計算の順番に注意してください。また、桁あふれしないようにlong型に
107: // castして計算しています。
108: GRservoMgr.asCurAngle[ucID] = GRservoMgr.asBgnAngle[ucID] +
109: (short)((long)( GRservoMgr.asReqAngle[ucID] – GRservoMgr.asBgnAngle[ucID] ) *
110: (long)GRservoMgr.usCurTime / (long)GRservoMgr.usReqTime );
111: }
112: // 制御時間をインクリメントします。
113: GRservoMgr.usCurTime++;
114: if( GRservoMgr.usReqTime < GRservoMgr.usCurTime ){
115: // 終了処理
116: GRservoMgr.usReqTime = 0;
117: GRservoMgr.usCurTime = 0;
118: }
119: }
120:
121: // サーボへの角度指示
122: // 計算結果（計算していない場合は前のままの値）をサーボに指示します。
123: sendAngles();
124: }
125:
126:
127: //
128: // メインルーチン
129: //
130: int main( void ){
131: short sStep; // ステップ
132: unsigned char ucID; // サーボID
133: unsigned long ulTimer; // 0.833msecタイマ記憶用
134:
135: RPU_InitConsole( br115200 ); // RPU-10ライブラリの初期化
136: SV_Init( br115200 ); // サーボ制御ライブラリの初期化
137: sei(); // 割り込み処理開始
138:
139: sStep = -1; // -1で停止と扱うので、事前初期化。
140: // どかっとサーボ管理データ初期化
141: memset( &GRservoMgr, 0x00, sizeof( TSRVM ));
142:
143: // １秒待つ（よく知らないけど必要らしい）
144: RPU_ResetTimerCounter();
145: while( RPU_GetTimerCounter10() < 100 );
146:
147: // 起動メッセージの表示
148: rs0_puts_P( PSTR( “MOVE 3 SERVOS LIKE MOTION PLAY\n” ));
149: rs0_puts_P( PSTR( “PLEASE INPUT KEY a=START, b=STOP.\n” ));
150:
151: // サーボ(31,32,33)トルクオン
152: for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
153: SV_TorqueOnOff( GRaucTBuff, ucID+31, 1 );
154: }
155:
156: // メインループ
157: // 20msec毎にループします。
158: while( 1 ){
159: // １ループが20msecになるように、あらかじめタイマー値を記憶します。
160: ulTimer = RPU_GetTimerCounter();
161:
162: // キー入力の受付
163: if( rs0_rx_buff() != 0 ){
164: switch( rs0_getc()){
165: case ‘a’: // 動作開始
166: // sStepを０にする（-1以外）ことで動作開始とします。
167: sStep = 0;
168: rs0_puts_P( PSTR( “START\n” ));
169: break;
170: case ‘b’: // ストップ
171: // ステップ動作を停止する。
172: sStep = -1;
173: // サーボ制御処理も新たな計算を停止させる。
174: GRservoMgr.usReqTime = 0;
175: GRservoMgr.usCurTime = 0;
176: rs0_puts_P( PSTR( “STOP\n” ));
177: break;
178: }
179: }
180:
181: // サーボ指示処理
182: // sStepが-1以外でかつ、sTimeが０のときに新しい角度指示を行います。
183: // 角度指示後、sTimeは20msec毎に減算されていき、０になったとき、
184: // また新しい角度指示を行います。
185: if( sStep != -1 ){
186: if( GRservoMgr.usReqTime == 0 ){
187: if( pgm_read_word( &GPposeList[sStep].usTime ) != 0 ){
188: // 指示情報表示
189: rs0_printf_P( PSTR( “STEP=%d ” ), sStep );
190: // 角度指示
191: GRservoMgr.usReqTime = pgm_read_word( &GPposeList[sStep].usTime );
192: rs0_printf_P( PSTR( “ReqTime=%d ” ), GRservoMgr.usReqTime );
193: for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
194: GRservoMgr.asReqAngle[ucID] = pgm_read_word( &GPposeList[sStep].asAngle[ucID] );
195: rs0_printf_P( PSTR( “ReqAngle[%d]=%d ” ), ucID+31, GRservoMgr.asReqAngle[ucID] );
196: }
197: rs0_printf_P( PSTR( “\n” ));
198: // nStepをインクリメントし、次回指示にそなえます。
199: sStep++;
200: }
201: else{
202: // 再生終了処理
203: // 設定データの動作時間が０なので、動作を完了します。
204: sStep = -1;
205: }
206: }
207: }
208:
209: // サーボ動作管理と指示
210: // この処理は常時20msec毎に呼び出され、角度指示を行います。
211: moveServo();
212:
213: // 20msec待ちます。
214: // 冒頭で保存した値と比較しますので、これでわりと正確に20msec待つ
215: // ことができます。RPU_GetTimerCounter()は、0.8333…msec単位の
216: // タイマカウンタです。そのため、20msecは24になります。
217: while( RPU_GetTimerCounter() < ( ulTimer + 24 ));
218: }
219:
220: return 1;
221: }
■今回で最後のハズだった！？RPU-10講座
ここで終わりにしようかなって思っていたんですが、大事なアイテムを忘れていました。それは「コントローラパッド」です。無線コントローラってやつですね。次回は、ばしばしっとコントローラパッドに迫ります。
※注意：本BLOGにてRPU-10での再プログラミングについての情報を公開していますが、これらはSISOが個人的に再プログラミングを行った時の技術情報を整理して紹介しています。GDLへのRPU-10ライブラリ同梱については、 Best Technologyさんのご好意で、趣味人への１つのチャンスとして同梱してくださっていると理解しています。そのため、RPU-10の再プログラミングについては、くれぐれもご自身の責任で、また、Best TechnologyさんやFUTABAさんに問い合わせたりすることの無いようにお願いいたします。
SiSO-LAB所長兼研究員のSiSOです。あ、趣味のサイトなので実体はありませんけど…。
[…] RPU-10講座／第十六章 ロングパケットでサーボ動作 […]