目次

• 始めに
• ヘッダファイル一覧
• 関数一覧
• 行動制御系関数
• 駆動系関数
• スレッド関数

1. 始めに

本ドキュメントは、MIRS標準ソフトウェア Ver2 で使用する関数について取りまとめたドキュメントである。標準ソフトウエア Ver.1 は、MIRSSTND-MANU-0002の関数レファレンスに従う。

2. ヘッダファイル一覧

Table.2 にヘッダファイルの一覧を示す。

Table.2 - ヘッダファイル一覧

ファイル名	利用される関数	説明
contro.h	行動制御系関数	行動制御プログラムで使われるマクロ、構造体、グローバル変数の定義。詳細はヘッダ内のコメントを参照
func_extern.h	行動制御系関数	行動制御プログラムで関数の extern 宣言集
pwm_enc.h	駆動系関数、行動制御系関数	主に行動制御系関数で使われるマクロ、構造体、グローバル変数の定義。一部、行動制御系関数でも利用される。詳細はヘッダ内のコメントを参照
pwm_module.h	pwm_enc_module2	PWM 走行制御に必要な制御パラメータの値を、マクロまたは構造体で与える。詳細はヘッダ内のコメントを参照
uss.h	超音波制御関連の関数	超音波距離計測の RT task に必要なパラメータの値を、マクロまたは構造体で与える。このバージョンから使用する IRQ が 5 から 11 に変更された。詳細はヘッダ内のコメントを参照
irs_ts_ws.h	行動制御系関数, irs_ts_ws	赤外線センサ、タッチセンサ、白線センサのデータをデバイスドライバ関連のパラメータ値をマクロで定義
irs_code.h	行動制御系関数, irs_code	赤外線センサのコード判別用デバイスドライバののパラメータをパラメータ値をマクロで定義

3.関数一覧

Table.2 に行動制御系関数の一覧を示す。

Table.2 - 行動制御系

ファイル名	関数名	書式	戻り値	説明
control_test.c	main	int main(void);	0	メイン
initialize.c	initialize	int initialize(void);	0	初期化
post_get.c	post_get	int post_get(int);	0	ポスト周回及び獲得
post_search.c	post_search	int post_search(int);	0	ポスト探索
post_check.c	post_check	void post_check(int);	0	ポスト確認
rm_data.c	rm_data	void rm_data(int *, int *, int *);	0	同一ポストデータ消去
circle_position.c	circle_position	void circle_postion(void);	0	周回半径への移動
post_approach.c	post_approach	int post_approach(int);	0 または 9999	ポスト接近
functions.c	post_select	int post_select(int);	-1 or 次のポスト番号	次ポスト選択
	move_center	int move_center(int);	0	ポスト周回後の中心位置復帰
	correct_direction1	int correct_direction1(void);	0	ポスト接近前の角度補正
	correct_direction2	int correct_direction2(void);	0	ポスト補正後の角度補正
	irs_code_get	int irs_code_get(int);	赤外線コード or 0	赤外線のコード判別
	uss_get	int uss_get(int);	測定データ	超音波による距離測定
correct_center.c	correct_center	int correct_center(void);	0	中心位置補正計算
	primary_pos	int primary_pos(int, int, int);	0	中心位置補正移動

Table.3 に駆動系関数の一覧を示す。

Table.3 - 駆動系

ファイル名	関数名	書式	戻り値	説明
both.c	pwm_enco_start	void pwm_enco_start(int);	なし	RTタスクスレッドを開始
	pwm_enco_stop	void pwm_enco_stop(void);	なし	RTタスクスレッドを停止
	pwm_enco_restart	void pwm_enco_restart(void);	なし	RTタスクスレッドを再開
encoder.c	encoder_data	void encoder_data(float *lm, float *rm, float *lv, float *rv, int *br);	なし	100[ms]ごとの左右のタイヤの回転距離,速度を返す
encoder2.c	encoder_data2	void encoder_data2(float *la, float *ra, int *br);	なし	100[ms]ごとの左右のタイヤの回転角度を返す
encoder3.c	encoder_data3	void encoder_data3(float *la, float *ra, int *br);	なし	左右のタイヤの回転角度の合計を返す
pwm.c	pwm_both_data	void pwm_both_data(int, int);	なし	Duty比指定移動
	pwm_straight	void pwm_straight(int, int);	なし	PID制御による移動
	pwm_rotate	void pwm_rotate(int);	なし	PID制御によるその場回転
	pwm_lqr_straight	void pwm_lqr_straight(int, int);	なし	LQR制御による直進移動
	pwm_lqr_rotate	void pwm_lqr_rotate(int, int);	なし	LQR制御によるその場回転
	pwm_lqr_circle	void pwm_lqr_circle(int, int, int);	なし	LQR制御によるポスト周回
functions.c	lqr_straight	int pwm_lqr_straight(int, int);	0	pwm_lqr_straightのマクロ
	lqr_rotate	int pwm_lqr_rotate(int, int);	0	pwm_lqr_rotateのマクロ
	lqr_circle	int pwm_lqr_circle(int, int, int);	0	pwm_lqr_circleのマクロ

Table.4にスレッド系関数の一覧を示す。

Table.4 - スレッド系

ファイル名	関数名	書式	戻り値	説明
thread_func.c	irs_ts_ws	void *irs_ts_ws(void *t);	なし	赤外線,タッチセンサ,白線センサ読み取りスレッド
	encoder	void *encoder(void *t);	なし	エンコーダデータ読み取りスレッド
	uss_read	void *uss_read(void *t);	なし	超音波センサ測定スレッド（連続読みとり）
	uss_read2	void *uss_read(void *t);	なし	超音波センサ測定スレッド（１回読みとり）

4. 行動制御系関数

int main(void)

MIRSSTNDのメイン関数。

int initialize(void)

プログラムで使用する変数の初期化とFIFOやスレッドの生成を行っている。これはmain()を実行したときに一番初めに実行される関数である。

int post_get(int target_post)

こ れはpostの獲得動作を行う関数であり、引数のtarget_postには次にとるべきポストの番号が入る。この関数はpostに対して正対した状態か ら始まる。まず90度回転し、その後ポスト周回を始める。周回中に赤外線を検出したらコード判別を行い、target_postと同じなら獲得動作に移 る。獲得後は、周回開始位置まで残りを周回する。もとの位置にもどってきたところでこの関数を抜ける。

int post_search(int center_count)

こ れはポストの探索をする関数であり、フィールド中央で360度回転しながら、すべてのポストの角度と距離を記録する。引数で渡すcenter_count が 1（すなわち最初に中心位置に移動したとき）は、そのデータを、post 構造体に入れ、center_count が 1より大きい場合（中心への移動が2回目以降の場合）は、そのデータを post_tmp 構造体に入れる。ポストの判定は測定データ列の極小値をポストとするため、連続して増加、減少するときに重みをつけ、極小値付近での重 みの得点が大きいものをポストと判断する。また、一旦ポストと判定されたもののうち、その角度が20度以下の場合は得点の大きい方を真のポストと認識する。なお、超音波センサーのデータは MIRS中心とポスト中心の間の距離に換算して、ポスト距離(post.dis, post_tmp.disとして）記録している。

void post_check(int center_count)

その場回転を行ない、post_search で見つかったポストを、前方の２つの超音波センサで、ポスト角度、ポストまでの距離を正確に記録する。また、post_search のデータで、同じポストや架空のポストであった場合は、rm_data を呼ぶことで、そのデータを削除する。center_count の値が 1 のときデータは、post 構造体に、1より多き場合は、post_tmp 構造体にポストデータを書き込む。

void rm_data(int *p1, (int *p2, (int *p3)

post_checkで確認されてた同一のポストデータを消去する関数。p1に配列の先頭番地()、p2に格納する配列の先頭番地、p3に削除する配列の要素番号を与える。

void circle_positon(void)

post_get のポストの周回半径に合わせ、MIRSの位置を決定する関数。

int post_approach(int target_code)

これはpost_selectで指定された次に取るべきポストへアプローチするための関数である。探索によって得られたポストまでの回転角度と距離を使っ て、ポストに正対できるように移動する。その際の角度補正は、接近前に correct_direction1 で、接近後に correct_direction2 で行う。ポスト から USS_MIN 離れたところで一旦停止し、circle_position により、ポスト回転半径上に停止する。正対補正により接近するポストが発見できない場合は、9999を返す。

int post_select(int center_count)

これは次に取るポストの判断を行う関数である。引数のcenter_countは一回目とそれ以降の判断をするためのものであり、一回目の探索では一番近いもの、それ以降は次に近いポスト番号を戻り値として返して終了する。

int move_center(int target_post)

これはポスト周回後の中心復帰移動の関数である。中心まで戻ってきて、引数で渡されたtarget_postによってどの角度、回転すればもとの座標軸の正面軸を向けるかを判断する。正面を向いたらこの関数は抜ける。

int correct_direction1(void) or int correct_direction2(void)

角度誤差を補正する関数。ポストまでの距離が二つとも同じになるときがポストの中心を向いているときであるから、前方の二つの超音波センサの測定データの差から左右の角度誤差の補正を行う。correct_direction1 では、グローバル変数 cor_dir に正対補正時に回転した角度を入れる。

int irs_code_get(int dumy)

これは赤外線センサのコードを読み取る関数である。引数であるdumyには使用する赤外線センサの番号が入る。戻り値として赤外線センサのコードが1〜6で返る。何もないときは0が返る。

int uss_get(int uss_no)

これは超音波センサによる距離測定をする関数である。引数のuss_noには使用する超音波センサの番号が入る。戻り値は測定データが[mm]で返る。反応しないときは9999である。

int correct_center(void)

これは中心位置に戻ったときの座標のズレを直す関数である。一回目の探索によって得られたポストの角度と位置を二回目以降の探索データと比較し、誤差をすくないと仮定し、x,y方向の誤差と角度の誤差を計算し、一回目の座標系に戻る。

int primary_pos(void)

correct_center()によって計算されたデータを下に、最短行動をとってもとの中心位置へ補正する関数である。引数はそれぞれ回転と直進のデータである。第一引数は一回目の回転角度、第二引数は直進距離、第三引数は回転角度である。

5. 駆動系関数

void pwm_enco_start(int period)

ロータリーエンコーダ・PWM制御用のRT-タスクをスタートさせるための関数。実行周期を[msec]単位で引数として与える。

void pwm_enco_stop(void)

ロータリーエンコーダ・PWM制御用のRT-タスクを停止させるための関数。

void pwm_enco_restart(void)

pwm_enco_stopで停止させたRT-タスクを再スタートさせるための関数。

void encoder_data(float *l_distance, float *r_distance, float *l_speed, float *r_speed, int *br)

FIFO3から左右のロータリーエンコーダのカウント値とフラグの状態を100[ms]の周期で読み込んで、左右のタイヤの移動距離/速度とフラグの状態をポインタを使って値を返す関数。
br という変数は、PID_MODE,LQR_MODEの制御の状態を示すものであり、0なら制御の途中であり、1なら制御が完了したことを示す （Duty_Modeは常に0である）。また、distanceとspeedの単位はそれぞれ[mm]と[mm/sec]である。

void encoder_data2(float *l_angle, float *r_angle, int *br)

FIFO3から左右のロータリーエンコーダのカウント値とフラグの状態100[ms]の周期で読み込み、100msごとの左右のタイヤの回転角度[deg]をポインタを使って値を返す関数。
brという変数は、PID_MODE,LQR_MODEの制御の状態を示すものであり、0なら制御の途中であり、1なら制御が完了したことを示す（Duty_Modeは常に0である）。

void encoder_data3(float *l_angle, float *r_angle, int *br)

FIFO3から左右のロータリーエンコーダのカウント値とフラグの状態100[ms]の周期で読み込み、左右のタイヤの回転角度[deg]の合計をポインタを使って値を返す関数。
brという変数は、PID_MODE,LQR_MODEの制御の状態を示すものであり、0なら制御の途中であり、1なら制御が完了したことを示す（Duty_Modeは常に0である）。

void pwm_both_data(int l_duty, int r_duty)

左右のDutyを直接[-127〜+127]で与える関数である。l_dutyが左のタイヤ、r_dutyが右のタイヤへのdutyである。単位は共に[-]であり、前進が＋、後進が−となっている。
MODEはDUTY_MODEである。

void pwm_straight(int l_distance, int r_distance)

P制御を行って左右のタイヤをそれぞれ指定した距離だけ進める関数である。l_distanceが左のタイヤの目標距離,r_distanceが右のタイヤの目標距離である。単位は共に[mm]であり、前進が＋、後進が−となっている。

void pwm_rotate(int angle)

P制御を行ってその場回転を行う関数である。回転角度をangle[deg]で与え、左回転が+、右回転が-となっている。

void pwm_lqr_straight(int distance, int time)

LQRモードの直進を行う関数である。移動距離をdistance[mm]、移動時間をtime[msec]で与える。前進が+、後進が-である。

void pwm_lqr_rotate(int angle, int time)

LQRモードでその場回転を行う関数である。回転角度をangle[deg]、回転時間をtime[msec]で与える。左回転が+、右回転が-である。

void pwm_lqr_circle(int radius, int angle, int time)

LQRモードで、半径rの円を描くように移動する関数である。回転半径をradius[mm]、回転角度をangle[deg]、移動時間をtime[msec]で指定する。左周りが+、右回りが-である。

int lqr_straight(int distance, int time)

pwm_lqr_straight()を使用する際のRTタスクのスタート,ストップや初期化などの一連の流れをまとめたマクロ関数。引数はpwm_lqr_straight()に同じであり、通常(制御の途中で停止させない場合)こちらのマクロ関数を使用する。

int lqr_rotate(int angle, int time)

pwm_lqr_rotate()を使用する際のRTタスクのスタート,ストップや初期化などの一連の流れをまとめたマクロ関数。引数はpwm_lqr_rotate()に同じであり、通常(制御の途中で停止させない場合)こちらのマクロ関数を使用する。戻り値は実際に回転した角度

int lqr_circle(int radius, int angle, int time)

pwm_lqr_circle()を使用する際のRTタスクのスタート,ストップや初期化などの一連の流れをまとめたマクロ関数。引数はpwm_lqr_circle()に同じであり、通常(制御の途中で停止させない場合)こちらのマクロ関数を使用する。

6. スレッド系関数

関数(スレッド)を呼ぶわけではなく、以下のようにスレッドを生成すればスレッドは独立して動作する。超音波データ読みとりのスレッド以外は、初期化関数(initial) より生成される。

void *irs_ts_ws(void *t)

赤外線、タッチセンサ、白線センサの 0/1（オン／オフ） のデータを usleep で指定された間隔で読み続けるスレッド。読み取ったデータは、グローバル変数 ts, irs, ws に格納される。

void *encoder(void *t)

ロータリエンコーダのデータを usleep で指定された間隔で読み続けるスレッド。読みとったデータは、タイヤの回転距離に変換され、グローバル変数 alw, arw に格納される。

void *uss_read(void *t)

超音波距離測定タスクの周期測定タスクで得られた最新の超音波距離測定データを読み続けるスレッド。読みとったデータは、グローバル変数 uss_data に格納される。

void *uss_read2(void *t)

超音波距離測定タスクの１ショットタスクで得られたデータを読むスレッド。読みとったデータは、グローバル変数 uss_data に格納される。１ショットタスクで、距離測定データ rtf1 に入るまでデータは更新されない。