目次

• 始めに
• 関数一覧
• 行動制御系関数
• 駆動系関数
• スレッド関数

1.始めに

本ドキュメントは、MIRSSTNDので使用する関数について取りまとめたドキュメントである。

2.関数一覧

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

table.1-行動制御系

ソースファイル名	関数名	書式	戻り値	説明
control.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_approach.c	post_approach	int post_approach(int);	0	ポスト接近
functions.c	post_select	int post_select(int);	-1 or 次のポスト番号	次ポスト選択
	move_center	int move_center(int);	0	ポスト周回後の中心位置復帰
	correct_distance	int correct_distance(void);	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.2に行動制御系関数の一覧を示す。

table.2-駆動系

ソースファイル名	関数名	書式	戻り値	説明
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.3にスレッド系関数の一覧を示す。

table.3-スレッド系

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

3.行動制御系関数

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は一回目の探索かそうでないかを判断するものである。これは一回目の探索を座標の基準とし、二回目以降は二回目以降の探索と一回目の探索を比較し、座標のズレを直すために必要となってくる。ポストの判定は測定データ列の極小値をポストとするため、連続して増加、減少するときに重みをつけ、極小値付近での重みの得点が大きいものをポストと判断する。

int post_approach(int target_post)

これはpost_selectで指定された次に取るべきポストへアプローチするための関数である。探索によって得られたポストまでの回転角度と距離を使って、ポストに正対できるように移動する。その際の補正はcorrect_direction2()やcorrect_distance()で行う。ポストから10cm離れたところで正対してこの関数を抜ける。

int post_select(int center_count)

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

int move_center(int target_post)

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

int correct_distance(void)

post_approach()内でポストに近づく時、前方の超音波を使用し、残り距離を補正する関数。

int correct_direction1(void) or int correct_direction2(void)

角度誤差を補正する関数。ポストまでの距離が二つとも同じになるときがポストの中心を向いているときであるから、前方の二つの超音波センサの測定データの差から左右の角度誤差の補正を行う。

int irs_code_get(int dumy)

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

int uss_get(int uss_no)

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

int correct_center(void)

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

int primary_pos(void)

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

4.駆動系関数

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()に同じであり、通常(制御の途中で停止させない場合)こちらのマクロ関数を使用する。

5.スレッド系関数

関数(スレッド)を呼ぶわけではなく、以下のようにスレッドを生成すればスレッドは独立して動作する。以下に超音波の例を示す。

pthread_t uss_task;
pthread_attr_t attr_uss;
struct sched_param sched_param;

pthread_attr_init(&attr_uss);
sched_param.sched_priority = 20;
pthread_attr_setschedpolicy(&attr_uss, SCHED_RR);
pthread_attr_setschedparam (&attr_uss, &sched_param);
pthread_create(&uss_task, &attr_uss, &uss_read, (void *)1);