Movado のしくみ
Movado は，基本系（FUND: fundamentals）・パラメタ系（PARA: parameters）・制御系（CONT: controller）・通信系（COMM: communicator）からなります．
基本系（FUND）は，メインプログラムのほかに，32 ビット固定小数点モジュール FIX からなります．メインプログラム main では，Movado の各系（PARA・CONT・COMM）を初期化したのち，制御サイクルごとのタイマ割り込み，およびシリアル通信割り込みの待機状態にはいります．また，32 ビット固定小数点モジュールは，32 ビット固定小数点（fix）データどうしの乗算・除算・加算・減算関数を外部に提供します．
パラメタ（PARA）系は，32 ビット固定小数点パラメタ領域 F・16 ビットパラメタ領域 W と，こららパラメタの初期化モジュール VAR からなります．たとえば，2 番モータの現在位置（POS）であれば F[0x0b][2] のように，特定モータの特定パラメタに直接アクセス（読み書き）することが可能になります．
制御系（cont）は，制御サイクルごとに呼び出される制御メインモジュール ACT と，そこからさらに呼び出されるモータ制御モジュール PID，台形制御モジュール CAM，動作キャリブレーションモジュール MMF からなります．ACT モジュールは，エンコーダから現在位置（POS）を読み取り，現在速度（VEL）を計測（アンチエイリアシング処理）します．リクエストがあれば，MMF や CAM を起動し，現在の制御サイクルでの目標位置（XPOS）を決定して，この目標位置に向けたモータ制御を PID で実行します．
通信系（comm）は，ホスト PC からの指令パケットを待ち受け，受信時にはそのパケットを処理し，必要に応じて返信をホスト PC に送信するモジュール PAC からなります．パケットの処理は，多くの場合，パラメタの読み書きとして実行されます．
このページの先頭に戻る
基本系 FUND
【main：メインプログラム】
// // fundamentals #include "sh7046f.h" // header for sh7046f #include "bios.c" // bios program for Pico-2
sh7046f.h は SH2 (7046F) のレジスタなどを定義します．bios.c は Pico-2 のハードウェア資源にアクセスするためのインタフェース関数などを定義します．
// program version #define VER 0x0103 // ex. 0x010f for 1.15
この Movado プログラムは Version 1.3 です．
// Pico-2 controls 4 axes in parallel #define NN 4 // number of axes (motors)
4 台のモータが制御対象となります．加えて 4 台のラジコン用サーボも制御対象となります．
// packet length #define PACLEN 6 // packet length in bytes
パケットは 6 バイト（固定長）となります．
// address of this pico2 (5bits) <- to be set by SW5-1 uchar ADDR; // communication address
この Pico-2 のアドレスです．main() の中で，SW5〜1 の状態が代入されます
// control cycle (DTf: period, FQf: frequency) #define DTf8 0x00004189 // 0.001s in s0824 format #define FQf 0x03e80000 // 1000Hz in s1616 format #define CMi 767 // CMT count for 4000Hz = 768*8P
DTf8 は制御周期 0.001[s] を s0824（整数部 8 ビット小数部 24 ビットの固定小数点形式）で表現したもの，FQf は制御周波数 1000[Hz] を s1616（整数部 16 ビット小数部 16 ビットの固定小数点形式）で表現したものです．CMi は汎用タイマモジュール CMT0 を 4000Hz に設定するための定数です．
// fix type typedef int fix; // fixed point s1616
32 ビット固定小数点データ fix は，見かけ上は int と同じになります．加減算は +, - の演算子で可能ですが，オーバーフロー・アンダーフローを考慮した加減算は FIX_add, FIX_sub として定義されています．また，乗除算には FIX_mul, FIX_div を用います．
//
// main of Movado
int main ()
{
void VAR_init(void), // parameter initializer
PAC_do(uchar*), // packet interpretor
ACT_do(void); // actuation process
// check if rebooted from WOVF (watch-dog timer overflow)
if (WDT_check()) {
LED_set(0x1f); // "help me" on LEDs
for (;;); // infinite loop
}
番犬タイマによって Movado が強制リセットされた場合，すべての LED を点灯して無限ループに入ります．LED が全点灯した状態でモータが制御されていない（脱力している）ならば，ソフトウェア上の重大なエラーが生じたことを意味します．番犬タイマをリセットするには，マニュアル=リセットするか，電源を再投入してください．
// init LED, address
LED_init();
LED_set(0x00);
SWT_init();
ADDR = SWT_get();
LED の消灯します．Pico-2 の SW5〜1 を読み取り，その値を ADDR に格納します．
// init variable block
VAR_init();
パラメタ（32 ビット固定小数点パラメタ・16 ビットパラメタ）を初期化します．
// initialization for PWM/RCS/ENC
PWM_init();
RCS_init();
ENC_init();
DC モータを駆動する PWM モジュール，ラジコン用サーボを駆動する RCS モジュール，エンコーダにアクセスする ENC モジュールを初期化します．
// SCI setup with address
// PACLEN=6 (1-addr5-n2, 0-inst3-msbs4,
// 0-hept7, 0-hept7, 0-hept7, 0-hept7 )
SCI_init(ADDR, PACLEN, (void *) PAC_do);
シリアル通信モジュール SCI を初期化します．アドレスに ADDR をもつパケット (uchar*) packet を受信すると，自動的に PAC_do(packet) を呼び出すようになります．
// CMT setup (cselB2=0x00, CMi=767)
// 1000Hz (control) -> 4000Hz (strobe) -> P/768 -> CMi=767
CMT0_init(0x00, CMi, ACT_do);
汎用タイマモジュール CMT0 を初期化します．4000Hz のサイクルで，ACT_do() を呼び出すようになります．これは 4 つのモータ軸を順番に制御するためのもので，各モータは 1000Hz の制御サイクルをもちます．
// WDT setup (csel=0x04: 2.6667ms=375Hz)
WDT_init(0x04);
番犬タイマモジュール WDT を初期化します．通常は 1ms ごとに（ACT_do() の中で）WDT_reset() が呼び出されますが，この呼び出しが 2.6667ms 以上ない場合，Movado は強制的にリセットされることになります．
// start event-loop
set_imask(0);
for (;;) {
}
set_imask(0) によって割り込みが許可されます．Movado が直接扱う割り込みは SCI（シリアル通信）と CMT0（汎用タイマ）です．これらの割り込みが発生すると，それぞれ SCI_do() と ACT_do が呼び出されます．
return 0;
}
【FIX: 32 ビット固定小数点モジュール】
// // FIX: fixed point calculation (s1616) // xxxx:xxxx:xxxx:xxxx.xxxx:xxxx:xxxx:xxxx (32bits) // | | | | // 2^15 2^0 2^-1 2^-16
fix あるいは s1616 は，整数部 16 ビット・小数部 16 ビットの固定小数点を意味します．表現可能な値の範囲は（およそ）-32768〜32768 で，分解能は 2^-16（約 0.000015259）です．
fix FIX_safe_s3216 (long long f64)
{
f64（s3216）の範囲をチェックし，オーバーフローがあれば表現可能な最大値に，アンダーフローがあれば表現可能な最小値に制限します．
if ((f64 & 0xffff80000000LL) == 0xffff80000000LL)
return f64 & 0xffffffffLL; // minus (normal)
else if (f64 & 0x800000000000LL)
return 0x80000000; // minus (out of range)
else if (f64 & 0x7fff80000000LL)
return 0x7fffffff; // plus (out of range)
else
return f64; // plus (normal)
}
fix FIX_safe_s4816 (long long f64)
{
f64（s4816）の範囲をチェックし，オーバーフローがあれば表現可能な最大値に，アンダーフローがあれば表現可能な最小値に制限します．
if ((f64 & 0xffffffff80000000LL) == 0xffffffff80000000LL)
return f64 & 0xffffffffLL; // minus (normal)
else if (f64 & 0x8000000000000000LL)
return 0x80000000; // minus (out of range)
else if (f64 & 0x7fffffff80000000LL)
return 0x7fffffff; // plus (out of range)
else
return f64; // plus (normal)
}
fix FIX_mul (fix f1, fix f2)
{
long long f64;
f64 = (((long long) f1) * ((long long) f2)) >> 16;
(s4816 * s4816 は s3232 となり) >> 16 は s3216 となります．
return FIX_safe_s3216(f64);
}
fix FIX_div (fix f1, fix f2)
{
long long f64;
f64 = (((long long) f1) << 16) / ((long long) f2);
(s4816 << 16 は s3232 となり) / s1616 は s4816 となります．
return FIX_safe_s4816(f64);
}
fix FIX_add (fix f1, fix f2)
{
long long f64;
f64 = ((long long) f1) + ((long long) f2);
s4816 + s4816 は s4816 となります．
return FIX_safe_s4816(f64);
}
fix FIX_sub (fix f1, fix f2)
{
long long f64;
f64 = ((long long) f1) - ((long long) f2);
s4816 - s4816 は s4816 となります．
return FIX_safe_s4816(f64);
}
パラメタ系 PARA
【F：32 ビット固定小数点パラメタ】
// // fix quads (32bit fixed point s1616, packed for 4 axes) #define NF 29 // number of fix quads
32 ビット固定小数点パラメタの配列 F は，29 種類 × 4 モータ = 合計 116 要素あります．これらパラメタは VAR_init() によって初期化されます．
fix F[NF][NN];
たとえば，0 番モータの KP（比例制御ゲイン）へは，F[0x12][0] あるいは F[KP][0] のようにアクセスできます．
#define FO_MOT 0 // offset for DC-motor quads
#define FO_RCS 5 // offset for RC-servo quads
// ( XPOS, XVEL, ACAM, VCAM, PCAM,
// RXPOS,RXVEL,RACAM,RVCAM,RPCAM )
モータとラジコン用サーボの台形速度制御は，同一の関数 CAM_do() で実行されます．関係するパラメタへのアクセスは，CAM_do() にオフセット FO_MOT, FO_RCS を指定することで，モータとラジコン用サーボを区別しています．
#define XPOS 0x00 // goal position #define XVEL 0x01 // velocity in CAM (in rad/s) #define ACAM 0x02 // acceleration in CAM #define VCAM 0x03 // max velocity in CAM #define PCAM 0x04 // goal position in CAM
XPOS は目標位置 [rad] で，書き込み可能です．通常は setpos 命令によって書き込みます．モータはこの目標位置に移動し，その位置を保持しようとします．他の 4 種類のパラメタは，台形速度制御モジュールの内部で使用されます．
#define RXPOS 0x05 // RCS goal position #define RXVEL 0x06 // RCS velocity in CAM (in tick/s) #define RACAM 0x07 // RCS acceleration in CAM #define RVCAM 0x08 // RCS max velocity in CAM #define RPCAM 0x09 // RCS goal position in CAM
RXPOS は目標位置 [tick] で，書き込み可能です．通常は setpos_rcs 命令によって書き込みます．ラジコン用サーボはこの目標位置に移動し，その位置を保持しようとします．RACAM, RVCAM は台形速度制御における加速度 [tick/s/s] と最大速度 [tick/s] で，書き込み可能です．通常は，それぞれ setacc_rcs, setvel_rcs 命令によって書き込みます．他のパラメタは台形速度制御モジュールの内部で使用されます．なお単位 tick は，0x24000000 を原点（中央）とし，およそ 0x04000000[tick] あたり 30[deg] の回転に相当する仮想単位です．
#define RXPOS_DEF 0x24000000 // default (center) for RXPOS #define RACAM_DEF 0x04000000 // default RACAM (approx 30deg/s/s) #define RVCAM_DEF 0x04000000 // RVCAM default (approx 30deg/s)
ラジコン用サーボ（オープン制御）の初期位置（ほぼ中央）と，加速度・最高速度の初期値です．
#define PMAR 0x0a // margin for MIN/MAX (in rad) #define PMAR_DEF 0x00010000 // default for PMAR at 1rad
モータ保護のための動作限界位置からの安全マージンです．モータの動作範囲は安全マージン分だけ減少します．PMAR の初期値は 1[rad] です．
#define POS 0x0b // current position #define VEL 0x0c // current velocity (after LPF) #define XTOR 0x0d // goal torque (EXPERIMENTAL) #define TOR 0x0e // current torque (EXPERIMENTAL)
POS は現在位置 [rad]，VEL は現在速度 [rad/s] で，ともに読み出し専用です．XTOR, TOR は現在未使用となっています．
#define ERPC8 0x0f // encoder resolution (s0824) #define ERPC8_16 0x001921fb // rad/count (for Nenc=16) #define ERPC8_200 0x000202b8 // rad/count (for Nenc=200) #define ERPC8_256 0x00019220 // rad/count (for Nenc=256)
エンコーダ分解能 [rad/count] を s0824（整数部 8 ビット・小数部 24 ビットの固定小数点）で表現したものです．3 つのデフォルト値候補があり，アドレス ADDR に応じて選択されます．（VAR_init() を参照）
#define MIN 0x10 // min position (set by MMF) #define MAX 0x11 // max position (set by MMF) #define MIN_DEF 0x80000000 // default MIN position #define MAX_DEF 0x7fffffff // default MAX position
モータの動作限界位置（負方向 MIN・正方向 MAX）です．初期値として，s1616 で表現できる最小値 -32768・最大値約 32768 が与えられいますが，動作キャリブレーション MMF_do() によって実際の動作限界位置が書き込まれます．
#define KP 0x12 // gain for proportional control #define KI 0x13 // gain for integral control #define KD 0x14 // gain for differential control #define KP_DEF 0x00020000 // default KP = 2.00000 #define KI_DEF 0x00280000 // default KI = 40.00000 #define KD_DEF 0x00000ccd // default KD = 0.05000
PID 制御のための比例制御ゲイン KP・積分制御ゲイン KI・微分制御ゲイン KD です．デフォルト値は 1〜5[W] 程度のコアレス DC モータ（Maxon RE シリーズなど）を想定したものになっています．
#define VMP 0x15 // motor power voltage (in V) #define VMP_DEF 0x000c0000 // VMP default (for 12.0V)
モータ駆動電圧（Ｈブリッジなどの入力電圧）です．
#define VMAG 0x16 // voltage magnifier
// VMAG = 32768 / VMP
モータに加える電圧（単位は V）を PWM_set() に与える 16 ビットデータに変換するための比例定数です．VAR_init() によって VMP から計算されます．
#define VINT 0x17 // accumlator for integral cont
積分制御の出力電圧で，モータ制御モジュールの内部で使用されます．
#define POLD 0x18 // old position for velocity calc
微分制御における速度計算のために制御メインモジュールの内部で使用されます．
#define LPFA 0x19 // alpha factor (for LPF(VEL)) #define LPFA_16 0x00001000 // alpha = 1/16 #define LPFA_200 0x00004000 // alpha = 1/4 #define LPFA_256 0x00004000 // alpha = 1/4
速度フィルタ係数（attack 係数）です．3 つのデフォルト値候補があり，アドレス ADDR に応じて選択されます．（VAR_init() を参照）
#define LPFD 0x1a // decay factor (for LPF(VEL))
// LPFD = 1.0 - LPFA
速度フィルタ係数（decay 係数）です．VAR_init() によって LPFA から計算されます．
#define ACAM0 0x1b // acceleration for move commands #define ACAM0_DEF 0x00d406ae // default ACAM0 = 0.5*(2*Pi*67.49)
move・movel 命令で使用する加速度（の絶対値）です．通常は setacc 命令によって書き込みます．
#define VCAM0 0x1c // max velocity for move commands #define VCAM0_DEF 0x00d406ae // default VCAM0 = 0.5*(2*Pi*67.49)
move・movac 命令で使用する最大速度（の絶対値）です．通常は setvel 命令によって書き込みます．
【W：16 ビットパラメタ】
// // word block (16bit; unsigned short) #define NW 10 // number of word quads
16 ビットパラメタの配列 W は，10 種類 × 4 モータ = 合計 40 要素あります．これらパラメタは VAR_init() によって初期化されます．
ushort W[NW][NN];
たとえば，2 番モータの CAMOD（台形速度制御の内部モード）へは，W[0x00][2] あるいは W[CAMOD][2] のようにアクセスできます．
#define WO_MOT 0 // offset for DC-motor quads
#define WO_RCS 2 // offset for RC-servo quads
// ( CAMOD, MODE,
// RCAMOD, RMODE )
モータとラジコン用サーボの台形速度制御は，同一の関数 CAM_do で実行されます．関係するパラメタへのアクセスは，CAM_do にオフセット WO_MOT, WO_RCS を指定することで，モータとラジコン用サーボを区別しています．
#define CAMOD 0x00 // submode in CAM #define MODE 0x01 // operation mode #define RCAMOD 0x02 // RCS submode in CAM #define RMODE 0x03 // RCS operation mode #define CAMOD_NO 0x0000 // no CAM operation #define CAMOD_GO 0xffff // start CAM with ACAM (move/ac/el) #define CAMOD_FF 0x000f // goal ahead, moving forward #define CAMOD_FB 0x00f0 // goal ahead, moving backward #define CAMOD_BF 0x0f00 // goal back, moving forward #define CAMOD_BB 0xf000 // goal back, moving backward
CAMOD, RCAMOD は台形速度制御の内部モードを表示します．0 以外の値は，台形速度制御による移動の最中であることを意味します．このパラメタに 0 を書き込むことで，台形速度制御をキャンセルすることができます．
#define MODE_NOP 0 // no operation #define MODE_POS 1 // position control #define MODE_VEL 2 // velocity control (reserved) #define MODE_TOR 3 // torque control (reserved)
MODE, RMODE は運転モードを表示します．デフォルトでは，すべてのモータが MODE_POS の運転モードになっています．MODE_NOP の場合は制御なし（脱力）となります．
#define MMFLG 0x04 // minmax finder flag #define MM_NOP 0x0000 // not in minmax mode #define MM_MIN 0xf000 // mining flag for MMF #define MM_MAX 0x000f // maxing flag for MMF #define MM_GO_CEN 0x03c0 // go-to-cen flag for MMF #define MM_GO_MIN 0x0c00 // go-to-min flag for MMF #define MM_GO_MAX 0x0030 // go-to-max flag for MMF
MMFLG は動作キャリブレーションの内部モードを表示します．0 以外の値は，動作キャリブレーションが実行中であることを意味します．この MMFLG に 0 を書き込むことで，動作キャリブレーションをキャンセルすることができます．
#define MMEMG 0x05 // minmax error magnitude
動作キャリブレーション時の限界位置検出係数です．do_min・do_max 系命令の引数 emg の値がこの MMEMG に格納され，動作キャリブレーション MMF_do() に利用されます．目標位置と現在位置の差（の絶対値）が，速度 VCAM あるいは RVCAM で MMEMG [ms] の時間で移動できる距離を超えた場合，MMF_do() は現在位置を動作限界点であると認識します．
#define REP_INT 0x06 // report interval (of Dt) #define REP_CNT 0x07 // report count (tmp) #define REP_ACT 0x08 // report count in action #define REP_TIC 0x09 // report interval tick
get_report 命令に対応するときに，内部的に使用されます．
【VAR：パラメタ初期化モジュール】
//
// initialize variables
void VAR_init ()
{
int n;
fix erpc8, lpfa;
// clear F/W blocks
for (n = 0; n < NN; n++) {
int index;
// clear fix block
for (index = 0; index < NF; index++) F[index][n] = 0;
// clear word block
for (index = 0; index < NW; index++) W[index][n] = 0;
}
まずは，パラメタ配列 F, W を 0 に初期化します．
// set encoder resolution and lpf parameters (according to ADDR)
if (ADDR & 0x10) { // 0x10..0x1e -> 256ppr/1000Hz
erpc8 = ERPC8_256;
lpfa = LPFA_256;
}
else if (ADDR & 0x08) { // 0x08..0x0f -> 200ppr/1000Hz
erpc8 = ERPC8_200;
lpfa = LPFA_200;
}
else { // 0x00..0x07 -> 16ppr/100Hz
erpc8 = ERPC8_16;
lpfa = LPFA_16;
}
つぎに，アドレス ADDR に応じて，3 種類のエンコーダ分解能（16, 200, 256[ppt]）に対応したパラメタを ERPC8（エンコーダ分解能）と LPFA（速度フィルタ係数）を選択します．
// init F/W blocks (for non-zero init value)
for (n = 0; n < NN; n++) {
// F
F[KP][n] = KP_DEF;
F[KI][n] = KI_DEF;
F[KD][n] = KD_DEF;
F[ACAM0][n] = ACAM0_DEF;
F[VCAM0][n] = VCAM0_DEF;
F[MIN][n] = MIN_DEF;
F[MAX][n] = MAX_DEF;
F[PMAR][n] = PMAR_DEF;
F[ERPC8][n] = erpc8;
F[LPFA][n] = lpfa;
F[LPFD][n] = FIX_sub(0x00010000, lpfa);
F[VMP][n] = VMP_DEF;
F[VMAG][n] = FIX_div(0x7fffffff, VMP_DEF);
F[RXPOS][n] = RXPOS_DEF;
F[RACAM][n] = RACAM_DEF;
F[RVCAM][n] = RVCAM_DEF;
// W
W[MODE][n] = MODE_POS;
W[RMODE][n] = MODE_POS;
}
0 以外のデフォルト値をもつパラメタを初期化します．なお，LPFD は 1.0 - LPFA として計算され，VMAG は 32768.0 / VMP として計算されます．動作モード MODE, RMODE は位置制御に設定されます．
}
制御系 CONT
【ACT：制御メインモジュール】
//
// ACT: actuation process
void ACT_do (void)
{
fix ACT_enc(uchar); // encoder read in rad
void PAC_report(int, fix), // position report for "report"
MMF_do(uchar), // minmax finder module
CAM_do(uchar, uchar, uchar), // trapezoid control module
PID_do(uchar); // PID control module
static uchar n = 0; // axis number {0,1,2,3}
fix pos, vel; // current pos, vel (tmp)
ACT_do() は 0.25ms（4000Hz）の周期で呼び出され，n 番モータの制御を行ないます．
// update POS and VEL
pos = ACT_enc(n);
F[POS][n] = pos;
vel = FIX_mul(FIX_sub(pos, F[POLD][n]), FQf);
F[POLD][n] = pos;
現在位置 POS をエンコーダから読み取り，前回の呼び出し時の位置 POLD との差に制御周波数 FQf（1000[Hz]）を乗じて，現在速度（フィルタ適用前）を求めます．
// low-pass filter for velocity
// (needed for stabilizing the differentiation)
// LPF = (1.0 - LPFA) * VEL + LPFA * VEL
F[VEL][n] = FIX_add(FIX_mul(F[LPFD][n], F[VEL][n]),
FIX_mul(F[LPFA][n], vel) );
速度フィルタを適用し，現在速度 VEL を求めます．このフィルタによりアンチエイリアシング処理を行ない，安定した微分制御（PID_do() の内部）を実現しています．
// report (if needed)
if (W[REP_ACT][n]) {
if (W[REP_TIC][n] == 0) {
PAC_report(n, pos); // report POS
W[REP_ACT][n]--; // decrement counter
W[REP_TIC][n] = W[REP_INT][n]; // reset interval
}
W[REP_TIC][n]--;
}
もし get_report 命令の実行中ならば，所定のインターバル回数（時間[ms]）ごとに PAC_report() により現在位置を PC に返信します．
// control the n-th motor
switch (W[MODE][n]) {
case MODE_NOP: // no-operator (and raw mode)
break;
case MODE_POS: // position control
// if minmax then
if (W[MMFLG][n]) MMF_do(n);
// if CAM then
if (W[CAMOD][n]) CAM_do(n, FO_MOT, WO_MOT);
// PID control
PID_do(n);
break;
デフォルトの位置制御では，まず，動作キャリブレーションがリクエストされている場合（MMFLG ≠ 0 のとき）MMF_do() を実行します．台形速度制御がリクエストされている場合（CAMOD ≠ 0 のとき）CAM_do() を実行します．MMF_do()・CAM_do() を実行した場合，あるいは setpos 命令を受信した場合，目標位置 XPOS が更新されます．そうでなければ，XPOS は直前の制御サイクルでの目標位置と同じ値となります．いずれの場合も，PID_do() によって目標位置 XPOS へのモータ制御を実行します．
case MODE_VEL: // velocity control (experimental)
PWM_set(n, FIX_mul(F[XVEL][n], F[VMAG][n]) >> 16);
break;
case MODE_TOR: // torque control (experimental)
break;
}
// control the n-th RCS
switch (W[RMODE][n]) {
case MODE_NOP: // no-operator (and raw mode)
break;
case MODE_POS: // position control
// if cam then
if (W[RCAMOD][n]) CAM_do(n, FO_RCS, WO_RCS);
// actuate the RCS
RCS_set(n, F[RXPOS][n] >> 16);
break;
ラジコン用モータについては，台形速度制御がリクエストされている場合（RCAMOD ≠ 0 のとき）CAM_do() を実行することで，あるいは setpos_rcs 命令を受信することで，目標位置 RXPOS を更新します．そうでなければ，RXPOS は直前の制御サイクルでの目標位置と同じ値となります．いずれの場合も，RCS_set() によって目標位置 RXPOS へラジコン用モータを駆動します．
}
// update counters
n++;
if (n == NN) {
// reset/update the counters
n = 0;
// clear watch-dog timer
WDT_reset();
}
モータ番号（およびラジコン用サーボ番号）を表わす静的変数 n を更新します．n が 0-1-2-3 と一巡するごとに WDT_reset() を呼び出し，番犬タイマに「正常運転中」を知らせます．
}
// fix ACT_enc ()
// returns encoder position in rad (radian)
// in fixed point s1616 format;
fix ACT_enc (uchar n)
{
// ENC_get(n): Encoder count in 24bit integer
// EPRC8[n] : Encoder resolution (rad/count; s0824)
// --------------- s1616 -----------------
// ----- s2408 ----- -- s0824 --
return FIX_mul((ENC_get(n) << 8), F[ERPC8][n]);
エンコーダの読み取り値（ENC_get() の値）は 24 ビットの整数です．これを s2408 表現に変換し，ERPC8（s0824）を乗じることで，s1616 表現の現在位置を求めます．
}
【PID：モータ制御モジュール】
//
// PID: controller
void PID_do (uchar n)
{
fix xposf, // goal position (after limitter)
erf, // error (s1616)
vpf, // output of proportional control
vdf, // output of differential control
vof; // final output
// limit range of motion in [MIN, MAX]
xposf = F[XPOS][n];
if (xposf < F[MIN][n]) xposf = F[MIN][n];
if (xposf > F[MAX][n]) xposf = F[MAX][n];
モータ動作の目標位置を [MIN, MAX] の範囲に制限します．
// calculate error
// erf = xpos - POSf
erf = FIX_sub(xposf, F[POS][n]);
目標位置と現在位置の差（エラー）です．このエラーをもとに，制御量を計算していきます．
// proportional control
// vpf = KPf * erf
vpf = FIX_mul(F[KP][n], erf);
比例制御ではエラーに比例したモータ電圧 vpf を求めます．
// integral control
// VINT += KIf * DTf * ERf
F[VINT][n] = FIX_add(F[VINT][n], // --- s2408 --- -s0824-
FIX_mul(FIX_mul((F[KI][n] >> 8), DTf8), erf));
if (F[VINT][n] > F[VMP][n]) F[VINT][n] = F[VMP][n];
if (F[VINT][n] < -F[VMP][n]) F[VINT][n] = -F[VMP][n];
積分制御ではエラーの積分値に比例したモータ電圧 VINT を求めます．ただし，VINT の絶対値はモータ駆動電源 VMP を超えないように制限します．
// differential control (position-based version)
// vdf = KDf * (POLD - POS) / DTf --- (DTf: DeltaT=0.001s)
// = KDf * (POLD - POS) * FQf --- (FQf: Freq=1/DeltaT)
// = - KDf * VEL
vdf = -FIX_mul(F[KD][n], F[VEL][n]);
微分制御ではエラーの微分に比例した制御を行なう流儀と，位置の微分（すなわち現在速度 VEL）に比例した制御を行なう流儀があり，Movado では後者を採用し，速度に比例したモータ電圧 vdf を求めています．ちょうど VEL に比例した動摩擦抵抗（粘性抵抗）と同じ働きをもちます．なお VEL は速度フィルタによるアンチエイリアシング処理後のものです．
// sum up the outputs
// vof = (vpf + VIf + vdf);
vof = FIX_add(vpf, FIX_add(F[VINT][n], vdf));
if (vof > F[VMP][n]) vof = F[VMP][n];
if (vof < -F[VMP][n]) vof = -F[VMP][n];
比例制御・積分制御・微分制御によるモータ電圧 vpf, VINT, vdf を足し合わせ，その絶対値がモータ駆動電源 VMP を超えないように制限します．
// power meter for debugging
if (vof >= 0)
LED_op(0x0f, (vof >> 16) & 0x0f);
else
LED_op(0x0f, ((-vof) >> 16) & 0x0f);
モータに与える電圧 vof を LED に表示しています．各モータについて 0.25ms ごとに切り替えて表示するため，見た目には 4 つのモータへの平均電圧が LED に表示されます．
// PWM output (s1616 -> short)
PWM_set(n, FIX_mul(vof, F[VMAG][n]) >> 16);
PWM_set() によってモータに電圧 vof を与えます．PWM_set() へは 16 ビット整数で電圧を指定します．-32768 のとき -VMP，32767 のとき VMP の電圧がモータに与えられます．
}
【CAM：台形制御モジュール】
// // CAM: constant acceleration mode (MOT/RCS shared version) // for MOT (motor), provide FO_MOT and WO_MOT for fo and wo; // for RCS (servo), provide FO_RCS and WO_RCS for fo and wo; // (these values will give bias for var-block index)
CAM_do() はモータ（MOT0〜3）とラジコン用サーボ（RCS0〜3）の両方について台形速度制御を行なうことができます．モータについての台形速度制御の場合，パラメタのオフセット量を fo = FO_MOT，wo = WO_MOT とします．ラジコン用サーボについての台形速度制御の場合には，fo = FO_RCS，wo = WO_RCS とします．
void CAM_do (uchar n, uchar fo, uchar wo)
{
fix acam, vcam, xvel;
acam = FIX_mul(FIX_mul(F[ACAM+fo][n] >> 8, DTf8) >> 8, DTf8);
vcam = FIX_mul(F[VCAM+fo][n] >> 8, DTf8);
xvel = F[XVEL+fo][n];
acam は制御サイクルあたりの加速度，vcam は制御サイクルあたりの最高速度，XVEL は制御サイクルあたりの（現在の）目標速度となります．
// ACAM0 should be more than 15.26 (=1000*1000/65536)
// (in any case, acam should be more than 0x00000001)
if (acam <= 0) acam = 0x00000001;
桁落ちのため acam が 0 になると加減速できなくなってしまうので，それを排除します．
// CAM operation
if (xvel > acam) {
// I'm moving forward
if (F[XPOS+fo][n] > F[PCAM+fo][n]) {
現在前進中（プラスの速度）かつ最終目標位置 PCAM が後方（現在目標位置 XPOS から見てマイナス方向）にある場合，行き過ぎなので，とにかくマイナス方向に速度を（最高速度の制限内で）減らします．
// and the goal is behind!
W[CAMOD+wo][n] = CAMOD_BF;
// decrease speed, anyway
xvel = FIX_sub(xvel, acam);
if (xvel < -vcam)
xvel = -vcam;
}
else {
現在前進中（プラスの速度）かつ最終目標位置 PCAM が前方（現在目標位置 XPOS から見てプラス方向）にある場合，減速開始位置 pcrif を計算し，もしそれを過ぎていたらマイナス方向に速度を（最高速度の制限内で）減らし，そうでなければプラス方向に速度を（最高速度の制限内で）増やします．
int vabsf, pcrif;
// the goal is ahead
// (or I'm passing above the goal forward)
W[CAMOD+wo][n] = CAMOD_FF;
// compute the critical point for braking
vabsf = xvel;
pcrif = FIX_sub(F[PCAM+fo][n],
FIX_div(FIX_mul(vabsf, FIX_add(vabsf, acam)),
FIX_add(acam, acam) ));
// If I have passed (or am passing) the point,
// then decrease speed
// otherwise increase speed
if (F[XPOS+fo][n] >= FIX_sub(pcrif, vabsf)) {
xvel = FIX_sub(xvel, acam);
if (xvel < -vcam)
xvel = -vcam;
}
else {
xvel = FIX_add(xvel, acam);
if (xvel > vcam)
xvel = vcam;
}
}
}
else if (xvel < -acam) {
// I'm moving backward
if (F[XPOS+fo][n] < F[PCAM+fo][n]) {
現在後進中（マイナスの速度）かつ最終目標位置 PCAM が前方（現在目標位置 XPOS から見てプラス方向）にある場合，行き過ぎなので，とにかくプラス方向に速度を（最高速度の制限内で）増やします．
// and the goal is ahead!
W[CAMOD+wo][n] = CAMOD_FB;
// increase speed, anyway
xvel = FIX_add(xvel, acam);
if (xvel > vcam)
xvel = vcam;
}
else {
現在後進中（マイナスの速度）かつ最終目標位置 PCAM が後方（現在目標位置 XPOS から見てマイナス方向）にある場合，減速開始位置 pcrif を計算し，もしそれを過ぎていたらプラス方向に速度を（最高速度の制限内で）増やし，そうでなければマイナス方向に速度を（最高速度の制限内で）減らします．
int vabsf, pcrif;
// the goal is behind
// (or I'm passing above the goal backward)
W[CAMOD+wo][n] = CAMOD_BB;
// compute the critical point for braking
vabsf = -xvel;
pcrif = FIX_add(F[PCAM+fo][n],
FIX_div(FIX_mul(vabsf, FIX_add(vabsf, acam)),
FIX_add(acam, acam) ));
// If I have passed (or am passing) the point,
// then increase speed
// otherwise decrease speed
if (F[XPOS+fo][n] <= pcrif + vabsf) {
xvel = FIX_add(xvel, acam);
if (xvel > vcam)
xvel = vcam;
}
else {
xvel = FIX_sub(xvel, acam);
if (xvel < -vcam)
xvel = -vcam;
}
}
}
else {
// I'm almost stand still (VNOW is almost zero)
if (F[XPOS+fo][n] < FIX_sub(F[PCAM+fo][n], acam)) {
現在速度がほとんど 0 の状態で，最終目標位置 PCAM がまだ前方（現在目標位置 XPOS から見てプラス方向）にある場合，プラス方向に速度を（最高速度の制限内で）増やします．
// the goal is ahead
W[CAMOD+wo][n] = CAMOD_FF;
// increase speed to move forward
xvel = FIX_add(xvel, acam);
if (xvel > vcam)
xvel = vcam;
}
else if (F[PCAM+fo][n] + acam < F[XPOS+fo][n]) {
現在速度がほとんど 0 の状態で，最終目標位置 PCAM がまだ後方（現在目標位置 XPOS から見てマイナス方向）にある場合，マイナス方向に速度を（最高速度の制限内で）減らします．
// the goal is behind
W[CAMOD+wo][n] = CAMOD_BB;
// decrease speed to move backward
xvel = FIX_sub(xvel, acam);
if (xvel < -vcam)
xvel = -vcam;
}
else {
現在速度がほとんど 0 の状態で，最終目標位置 PCAM がほぼ現在目標位置 XPOS と重なる場合，XPOS に PCAM を代入し，目標速度を 0 にして，台形速度制御を終了します．
// I'm almost at the goal
// (force to stop moving at the goal)
F[XPOS+fo][n] = F[PCAM+fo][n];
xvel = 0;
W[CAMOD+wo][n] = CAMOD_NO;
return;
}
}
// finally update the goal position and goal velocity
F[XPOS+fo][n] = FIX_add(F[XPOS+fo][n], xvel);
F[XVEL+fo][n] = xvel;
計算された目標速度 xvel で現在目標位置 XPOS を更新します．
}
【MMF：動作キャリブレーションモジュール】
//
// MMF: minmax finder
void MMF_do (uchar n)
{
fix mvel, merr;
// indicate MMF-ing
LED_op(0x10, 0x10);
動作キャリブレーション中は LED5 を点灯させます．
// minmax velocity (VCAM0/FQf)
// (for different velocity, change setvel before do_min/max/etc)
mvel = FIX_mul(F[VCAM0][n] >> 8, DTf8);
動作限界点をみつけるための制御サイクルごとの移動量です．
// error for collision detection
// (ushort MMEMG [rad])
merr = FIX_mul(mvel, W[MMEMG][n] << 16);
目標位置と現在位置の差が merr を超えたとき，現在位置が動作限界点であると判定されます．逆に言えば，この差が merr を越えなければ，モータはまだ回転していると判定されます．merr は，モータが速度 VCAM0 で MMEMG [ms] の時間に移動する距離となります．MMEMG は動作キャリブレーション指令（do_min・do_max など）の emg として与えられたものです．
if (W[MMFLG][n] & MM_MIN) {
MM_MIN がリクエストされている場合，マイナス方向の動作限界点まで移動し，安全マージン PMAR 分だけ手前の位置を MAX に格納します．
// find min end
// if in CAM, stop it (otherwise XPOS would get stuck)
W[CAMOD][n] = CAMOD_NO;
// check collision with min end
if (-FIX_sub(F[XPOS][n], F[POS][n]) > merr) {
// collision!!
// set MIN at POS+margin
F[MIN][n] = FIX_add(F[POS][n], F[PMAR][n]);
// stop motor at MIN
F[XPOS][n] = F[MIN][n];
// clear MM_MIN flag
W[MMFLG][n] &= ~(MM_MIN);
LED_op(0x10, 0x00);
}
else {
// no collision, so go backward
F[XPOS][n] = FIX_sub(F[XPOS][n], mvel);
}
}
else if (W[MMFLG][n] & MM_MAX) {
MM_MAX がリクエストされている場合，プラス方向の動作限界点まで移動し，マージン PMAR 分だけ手前の位置を MAX に格納します．
// find max end
// if in CAM, stop it (otherwise XPOS would get stuck)
W[CAMOD][n] = CAMOD_NO;
// check collision with max end
if (FIX_sub(F[XPOS][n], F[POS][n]) > merr) {
// collision!!
// set MIN at POS-margin
F[MAX][n] = FIX_sub(F[POS][n], F[PMAR][n]);
// stop motor at MAX
F[XPOS][n] = F[MAX][n];
// clear MM_MAX flag
W[MMFLG][n] &= ~(MM_MAX);
LED_op(0x10, 0x00);
}
else {
// no collision, so go forward
F[XPOS][n] = FIX_add(F[XPOS][n], mvel);
}
}
else if (W[MMFLG][n] & (MM_GO_CEN | MM_GO_MAX | MM_GO_MIN)) {
MM_GO_* がリクエストされている場合，中央・プラス限界点・マイナス限界点へ移動し，その位置を原点（位置 0）とします．
fix xpos, diff;
// set goal position
if (W[MMFLG][n] & MM_GO_MAX)
xpos = F[MAX][n];
else if (W[MMFLG][n] & MM_GO_MIN)
xpos = F[MIN][n];
else // center = (MIN+MAX)*0.5
xpos = FIX_mul(FIX_add(F[MIN][n], F[MAX][n]), 0x00008000);
// forward/backword/terminte
diff = FIX_sub(xpos , F[XPOS][n]);
if (diff > mvel) {
// move forward
F[XPOS][n] = FIX_add(F[XPOS][n], mvel);
}
else if (diff < -mvel) {
// move backward
F[XPOS][n] = FIX_sub(F[XPOS][n], mvel);
}
else {
目標に移動できたら，その位置を原点（位置 0）とし，エンコーダを 0 にリセットし，この原点移動に合わせて MIN・MAX をずらします．
fix offset;
// zero (make current position "zero")
offset = xposf;
F[XPOS][n] = 0;
F[POS][n] = 0;
F[POLD][n] = 0;
ENC_set(n, 0);
// move MIN/MAX
F[MIN][n] = FIX_sub(F[MIN][n], offset);
F[MAX][n] = FIX_sub(F[MAX][n], offset);
// clear the flag
W[MMFLG][n] = MM_NOP;
LED_op(0x10, 0x00);
}
}
}
通信系 COMM
【PAC：パケット通信モジュール】
//
// PAC: packet translator
void PAC_to_NIQ (uchar *packet, ushort *n, ushort *inst, uint *quad)
{
6 バイトのパケット packet を，モータ番号 n，命令 inst，4 バイト引数 quad に変換（parse）します．
uchar msbs;
uchar data1, data2, data3, data4;
// motor number (N)
*n = packet[0] & 0x03;
// inst (I) from upper 3 bits of packet[1]
// (lower 4 bits are msbs for arg(g1-4))
*inst = packet[1] >> 4;
msbs = packet[1] & 0x0f;
// quad (Q) = data1..4
data1 = packet[2] | ((msbs & 0x08)? 0x80: 0);
data2 = packet[3] | ((msbs & 0x04)? 0x80: 0);
data3 = packet[4] | ((msbs & 0x02)? 0x80: 0);
data4 = packet[5] | ((msbs & 0x01)? 0x80: 0);
*quad = (data1 << 24) | (data2 << 16) | (data3 << 8) | data4;
}
void PAC_from_NIQ (uchar *packet, ushort n, ushort inst, uint quad)
{
モータ番号 n，命令 inst，4 バイト引数 quad に変換（parse）から，6 バイトのパケットを構成し，packet に格納します．
packet[0] = (ADDR << 2) | (n & 0x03);
packet[1] = (inst & 0x07) << 4
| ((quad & 0x80000000)? 0x08: 0)
| ((quad & 0x00800000)? 0x04: 0)
| ((quad & 0x00008000)? 0x02: 0)
| ((quad & 0x00000080)? 0x01: 0);
packet[2] = (quad & 0x7f000000) >> 24;
packet[3] = (quad & 0x007f0000) >> 16;
packet[4] = (quad & 0x00007f00) >> 8;
packet[5] = (quad & 0x0000007f);
}
// PAC_do: packet interpreter
ushort Low16; // lower 16bits for fix
32 ビット固定小数点パラメタの書き込みは，まず set_lower で下位 16 ビットを Low16 に格納し，後続する set_upper で特定 index をもつパラメタに上位 16 ビットと下位 16 ビット（Low16）を同時に書き込みます．パラメタ読み出しは，ます get_upper で特定 index をもつパラメタの上位 16 ビットを読み出すと同時に下位 16 ビットを Low16 に格納し，後続する get_lower で Low16 を読み出します．
void PAC_do (uchar *packet)
{
ushort n, inst;
uint quad;
uchar r_packet[PACLEN];
void PAC_report_trigger(int);
PAC_to_NIQ(packet, &n, &inst, &quad);
if (inst == 0) {
move 命令です．設定済みの加速度 ACAM0 と最高速度 VCAM0 を用いた台形速度制御を実行します．
// move: move to pos in CAM with preset acc
F[ACAM][n] = F[ACAM0][n];
F[VCAM][n] = F[VCAM0][n];
F[PCAM][n] = (fix) quad;
W[CAMOD][n] = CAMOD_GO;
// if needed, trigger reporting
if (W[REP_CNT][n]) PAC_report_trigger(n);
もし（get_report 命令によって）位置レポートが要求されている場合，move 命令は位置レポートの繰り返し返信を開始します．以下の movac, movel, setpos 命令の処理についても同様です．
}
else if (inst == 1) {
fix arg;
arg = (fix) quad;
if (arg < 0) {
movac 命令です．加速度は -upper，最高速度は VCAM0，目標位置は lower として台形速度制御を実行します．
// movac: move to pos in CAM with -acc
// (arg = acc:n16,pos:s16)
F[ACAM][n] = - (arg & 0xffff0000);
F[VCAM][n] = F[VCAM0][n];
F[PCAM][n] = (fix) (arg << 16);
W[CAMOD][n] = CAMOD_GO;
// if needed, trigger reporting
if (W[REP_CNT][n]) PAC_report_trigger(n);
}
else {
setacc 命令です．加速度 ACAM0 を設定します．
// setacc: acceleration for move
F[ACAM0][n] = arg;
}
}
else if (inst == 2) {
movel 命令です．加速度は ACAM0，最高速度は -upper，目標位置は lower として台形速度制御を実行します．
fix arg;
arg = (fix) quad;
if (arg < 0) {
// movel: move to pos in CAM with -vel
// (arg = vel:n16,pos:s16)
F[ACAM][n] = F[ACAM0][n];
F[VCAM][n] = - (arg & 0xffff0000);
F[PCAM][n] = (fix) (arg << 16);
W[CAMOD][n] = CAMOD_GO;
// if needed, trigger reporting
if (W[REP_CNT][n]) PAC_report_trigger(n);
}
else {
setvel 命令です．最高速度 VCAM0 を設定します．
// setvel: velocity for move
F[VCAM0][n] = arg;
}
}
else if (inst == 3) {
setpos 命令です．目標位置を quad とします．
// setpos: ballistic motion
// cancel CAM
W[CAMOD][n] = CAMOD_NO;
// set to XPOS
F[XPOS][n] = (fix) quad;
// if needed, trigger reporting
if (W[REP_CNT][n]) PAC_report_trigger(n);
}
else if (inst == 5) {
getacc 命令です．現在設定されている加速度を返信します．
// getacc: current acceleration
quad = (uint) F[ACAM][n];
PAC_from_NIQ(r_packet, n, inst, quad);
SCI_transmit(r_packet);
}
else if (inst == 6) {
getvel 命令です．現在速度を返信します．
// getvel: current velocity
quad = (uint) F[VEL][n];
PAC_from_NIQ(r_packet, n, inst, quad);
SCI_transmit(r_packet);
}
else if (inst == 7) {
getpos 命令です．現在位置を返信します．
// getpos: current position
quad = (uint) F[POS][n];
PAC_from_NIQ(r_packet, n, inst, quad);
SCI_transmit(r_packet);
}
else if (inst == 4) {
inst = 4 は副指令です．
ushort jnst, index, word;
// sub-command
jnst = quad >> 24;
index = (quad & 0x00ff0000) >> 16;
word = quad & 0x0000ffff;
if (jnst == 0x00) {
32 ビット固定小数点パラメタに書き込む set_lower, set_upper 命令です．VMP, LPFA への書き込みによって，VMAG, LPFD へも変換処理された値が書き込まれます．
// set_fix
if (index == 0xff)
Low16 = word;
else if (index < NF) {
F[index][n] = (fix) ((word << 16) | Low16);
// auto set variables (VMAG, LPFD)
if (index == VMP) {
// VMAG = 32768 / VMP;
F[VMAG][n] = FIX_div(0x7fffffff, F[VMP][n]);
}
else if (index == LPFA) {
// LPFD = 1.0 - LPFA
F[LPFD][n] = FIX_sub(0x00010000, F[LPFA][n]);
}
}
}
else if (jnst == 0x01) {
16 ビットパラメタに書き込む set_word 命令です．
// set_word
if (index < NW)
W[index][n] = word;
}
else if (jnst == 0x70) {
ラジコン用サーバの制御命令群です．
switch (index) {
case 0x00:
// move_rcs
F[RPCAM][n] = (fix) (word << 16);
W[RCAMOD][n] = CAMOD_GO;
break;
case 0x01:
// setacc_rcs
F[RACAM][n] = (fix) (word << 16);
break;
case 0x02:
// setvel_rcs
F[RVCAM][n] = (fix) (word << 16);
break;
case 0x03:
// setpos_rcs
W[RCAMOD][n] = CAMOD_NO;
F[RXPOS][n] = (fix) (word << 16);
break;
}
}
else if (jnst == 0x7f) {
その他の（返信を要しない）命令です．
switch (index) {
case 0x00: // set_zero
W[CAMOD][n] = CAMOD_NO;
F[XPOS][n] = 0;
F[POS][n] = 0;
F[POLD][n] = 0;
ENC_set(n, 0);
break;
case 0x01: // set_mode
W[MODE][n] = word;
break;
case 0x02: // set_camod
W[CAMOD][n] = word;
break;
case 0x04: // go_cen
W[MMFLG][n] = MM_GO_CEN;
break;
case 0x05: // do_max
W[MMFLG][n] = MM_MAX;
W[MMEMG][n] = word;
F[MAX][n] = MAX_DEF;
break;
case 0x06: // do_min
W[MMFLG][n] = MM_MIN;
W[MMEMG][n] = word;
F[MIN][n] = MIN_DEF;
break;
case 0x07: // do_minmax
W[MMFLG][n] = (MM_MIN | MM_MAX);
W[MMEMG][n] = word;
F[MIN][n] = MIN_DEF;
F[MAX][n] = MAX_DEF;
break;
case 0x08: // do_minmaxcen
W[MMFLG][n] = (MM_MIN | MM_MAX | MM_GO_CEN);
W[MMEMG][n] = word;
F[MIN][n] = MIN_DEF;
F[MAX][n] = MAX_DEF;
break;
case 0x09: // do_minmaxmax
W[MMFLG][n] = (MM_MIN | MM_MAX | MM_GO_MAX);
W[MMEMG][n] = word;
F[MIN][n] = MIN_DEF;
F[MAX][n] = MAX_DEF;
break;
case 0x0a: // do_minmaxmin
W[MMFLG][n] = (MM_MIN | MM_MAX | MM_GO_MIN);
W[MMEMG][n] = word;
F[MIN][n] = MIN_DEF;
F[MAX][n] = MAX_DEF;
break;
case 0xff: // reset
VAR_init();
for (n = 0; n < NN; n++)
ENC_set(n, 0);
break;
}
}
else if (jnst == 0x80) {
32 ビット固定小数点パラメタを読み出す get_lower, get_upper 命令です．
// get_fix F[index][n]
if (index == 0xff)
quad = (quad & 0xffff0000) | Low16;
else if (index < NF) {
uint tmp;
tmp = (uint) F[index][n];
Low16 = tmp & 0xffff;
quad = (quad & 0xffff0000) | (tmp >> 16);
}
else
quad = 0xffffffff; // to indicate error
PAC_from_NIQ(r_packet, n, inst, quad);
SCI_transmit(r_packet);
}
else if (jnst == 0x81) {
16 ビットパラメタを読み出す get_word 命令です．
// get_word W[index][n]
if (index < NW)
quad = (quad & 0xffff0000) | W[index][n];
else
quad = 0xffffffff; // to indicate error
PAC_from_NIQ(r_packet, n, inst, quad);
SCI_transmit(r_packet);
}
else if (jnst == 0x82) {
このプログラムのバージョンを返します．
// get_version
quad = VER;
PAC_from_NIQ(r_packet, n, inst, quad);
SCI_transmit(r_packet);
}
else if (jnst == 0xf0) {
ラジコン用サーボの制御パラメタを読み出します．
switch (index) {
case 0x05:
// getacc_rcs
word = (ushort) (F[RACAM][n] >> 16);
quad = (jnst << 24) | (index << 16) | word;
break;
case 0x06:
// getvel_rcs
word = (ushort) (F[RVCAM][n] >> 16);
quad = (jnst << 24) | (index << 16) | word;
break;
case 0x07:
// getpos_rcs
if (W[RCAMOD][n])
word = (ushort) (F[RPCAM][n] >> 16);
else
word = (ushort) (F[RXPOS][n] >> 16);
quad = (jnst << 24) | (index << 16) | word;
break;
default:
quad = 0xffffffff;
}
PAC_from_NIQ(r_packet, n, inst, quad);
SCI_transmit(r_packet);
}
else if (jnst == 0xff) {
get_report 命令です．index [ms] ごとに word 回だけの位置レポートを要求します．後続する move, movac, movel, setpos 命令によって，位置レポートが開始されます．
// get_report
W[REP_INT][n] = index; // interval (# of control loops)
W[REP_CNT][n] = word; // report counts
}
else if (jnst >= 0x80) {
未知の副指令（ただし jnst > 0x80）の場合，エラーパケットを返信します．
// unknown query (return "error" packet)
quad = 0xffffffff;
PAC_from_NIQ(r_packet, n, inst, quad);
SCI_transmit(r_packet);
}
}
}
void PAC_report_trigger (int n)
{
モータ n についての位置レポートを開始します．
W[REP_ACT][n] = W[REP_CNT][n];
W[REP_CNT][n] = 0;
W[REP_TIC][n] = 0;
}
void PAC_report (int n, fix pos)
{
モータ n の現在位置を返信します．PAC_report() は，ACT_do() の中から呼び出されます．
uchar r_packet[PACLEN];
PAC_from_NIQ(r_packet, n, 7, pos); // report packet (I=7)
SCI_transmit(r_packet);
}