沼津高専 電子制御工学科
MIRS0805　ソフトウェア製造仕様書 MIRS0805-SOFT-0002
改訂記録
版数	作成日	作成者	承認	改訂内容
A01	2009/02/13	塚本・芦澤	石井	初版

目次

１． 目的

２． 動作概要

３． 前提

４． 動作の流れ

５． ヘッダファイル一覧

６． 関数一覧

７． グローバル変数一覧

---

目的

本ドキュメントは、MIRS0805ソフトウェア製造仕様書である。

動作概要

競技１回目

１． 競技場中心でポスト探索。

２． 探索結果よりアプローチするポストを決定し、ポストに直線走行で移動する。

３． 周回動作によりポストの獲得、同時に探索されていないポストの探索。

４． ポストが全て獲得できるまで２、３の繰り返し。

競技2回目

１． 競技１回目の結果より、獲得するポストを決定する。

２． 決めたポストに向かって直線走行でアプローチする。

３． そのポストが獲得対象なら、普通に獲得。経由対象なら逆回りで周回。

４． 1〜3を繰り返すことで、順番どおりに全て獲得する。

前提

l MIRSの方向はコンパスからのデータを使用する。

l 直線走行は、カーブせず真っ直ぐ走れていると仮定する。このためパラメータ調整と重心調整は重要である。

l 直線走行した距離は、超音波センサを使用する。

l 移動は、すべて基準点（競技場中心か、ポストの中心位置）の間を直線移動する。

l 直線走行した距離と、コンパスからの方位によって算出したx,y移動量を、移動前の基準点の座標に足して目標位置の座標を求める。

動作の流れ

競技1回目(main1)

競技2回目(main2)

ヘッダファイル一覧

ファイル名	概要
mirs_head.h	標準でインクルードしているヘッダ郡がまとめてある。
define.h	使っているマクロ定数や、グローバル変数のextern宣言がされている。グローバル変数の本宣言はinitialize()内で行われている。

関数一覧

ファイル名	関数名	書式	概要
main1.c	main1	int main1(void);	競技一回目のメイン関数
main2.c	main2	int main2(void);	競技二回目のメイン関数
initialize.c	initialize	int initialize(void);	初期化
read_compass.c	read_compass	int read_compass(int cor_flag);	コンパスの値を読む
	set_led	void set_led(void);	LEDドライバにledの発光パターンのデータを送る
	port_open	void port_open(void);	RS-232Cポートをオープン
	port_close	void port_close(void);	RS-232Cポートをクローズ
read_compass_thread.c	read_compass_thread	void *read_compass_thread(void*);	copmpass_dataを更新するスレッド
post_search1.c	post_search1	void post_search1(void);	ポスト探索
xy_convert.c	xy_convert	void xy_convert(int tmp_flag);	座標変換
post_select1.c	post_select1	int post_select1(void);	ポスト選択(中心でのポスト探索終了後)
post_approach.c	post_approach	int post_approach(int post_ID,int cor_flag);	ポスト接近
circle_position.c	circle_position	void circle_position(int target_post);	周回半径への調整
post_get.c	post_get	void post_get(int post_ID,int restart_flag);	ポスト周回及び獲得
ir_check_thread.c	ir_check_thread	void *ir_check_thread(void* post_ID);	post_getから起動されるスレッド
post_check1.c	post_check1	void post_check1(void);	ポスト確認
post_select2.c	post_select2	int post_select2(int post_ID);	ポスト選択(ポスト周回後)
post_juyu.c	post_juyu	int post_juyu(int now_id);	ポスト選択(獲得済みポストにアプローチしたとき)
save_data.c	save_data	void save_data(void);	テーブル変数の内容を保存
functions.c	accident_check	int accident_check(int target_direc,int uss_restart_flag);	ポスト衝突後、復帰する
	illuminate	void illuminate(void);	LED発光
	color_set	void color_set(int r,int g,int b);	LEDのセット
	number_set	void number_set(int num,int mode);	LED発光
	wait_breath	int wait_breath(void);	待機状態
	correct_direction1	int correct_direction1(void);	ポスト接近前の角度補正
	correct_direction2	int correct_direction2(void);	ポスト補正後の角度補正

競技2回目用関数

read_data.c	read_data	void read_data(void);	ファイルから内容をテーブル変数にロード
post_select3.c	post_select3	int post_select3(int now_id);	ポスト選択

○関数詳細概要

int main1(void);

競技一回目のメイン関数。メイン関数から、各動作をする関数を呼び出す。

メイン関数内で行うのは　last_posの設定、ログ記録、ポスト位置データ（二回目で使う）をファイルに保存。ログとポスト位置データは、こまめに追加モードでのファイルオープン・クローズを繰り返して強制終了してもそこまでのデータが残るようにする。

int read_compass(int cor_flag);

単発でコンパスの値を読みcor_flag=0で生データを、１でdirec0による補正後のデータを返す。２回連続でやらないと今の値にならないから注意。最速10ms間隔くらいで実行できる。

void set_led(void);

LEDドライバにledの発光パターンのデータを送る。

void port_open(void);

I2Cマスタ用にRS-232Cポートをオープンし、I2Cマスタの初期設定を行う。

void port_close(void);

I2Cマスタ用ポートをクローズし、read_compass_threadを終了する。

void *read_compass_thread(void*);

read_compass()を実行し、戻り値でcopmpass_dataを更新するスレッド。led_change_flagをチェックし、変わってたらled_setを呼び出す。

void post_search1(void);

中心でのサーチ（その場回転）MIRS0701のやつをコンパス使うように改造したもの
 分解能を２[deg]とし、超音波センサ値の単発エラーを修正する機能を追加。
 tmp_post_direcにサーチ開始方位を記録する。

void xy_convert(int tmp_flag);

tmp_post_ang , tmp_post_dis , last_pos からxy座標を計算してtmp_post_x , tmp_post_yに代入する。tmp_flagが０のときは中心サーチなのでポストの半径を入れる。tmp_flagが１のときはポストを周回しながらのサーチなのでポストの半径、MIRSの周回半径なども計算に入れる。

int post_select1(void);

中心サーチのあと用。一番近いポストのポストIDを戻り値で返し、コンパスを使用してそのポストのほうを向く。

int post_approach(int post_ID,int cor_flag);

アプローチはつぎの４ステップで構成される。

１．正対補正（correct_direction1)

２．直進

３．正対補正(correct_direction2)

４．周回半径の調整

各ステップで、移動方位と移動距離から、MIRSの現在位置を計算しapp_post_x　app_post_yを更新することで、実際にアプローチしたポストを判別する。

アプローチする対象のポストIDを引数にとる。

cor_flagが１だったら実際にアプローチしたと思われるポストIDを戻り値で返し、実際にアプローチしたポストがデータになかったら新しく追加する。

cor_flagが０だったら引数で渡されたポストIDをそのまま返し、ポストの追加は行わない。

○例外処理（例外が発生しても、last_posを更新し、アプローチ位置計算は継続して行われる）

アプローチ中は前方の超音波センサを監視し、片方でもポストをロストしたらその場で停止した後、アプローチをやり直す。

アプローチ開始時の正対補正でポストが遠すぎて超音波センサの値が取れかった場合、ポスト位置データから算出した予想走行距離の半分を走行し、そこからアプローチをやり直す。これをバイオレットモードと呼んでいる。

バイオレットモードでは、前方の超音波センサを監視している。ポストが衝突コースにあることを検知したらすぐに停止し、そこからアプローチをやり直す。

アプローチ開始時の正対補正で競技場外のものへ補正してしまった場合は、target_direcの方向へ40度多めに向き直り、正対補正をやり直す。

void circle_position(int target_post);

post_approachの中で最後に使われる関数。設定されたポストの周回半径に合うような位置に調整する。

void post_get(int post_ID,int restart_flag);

サーチの開始方位（進入方向の真後ろ）をtmp_post_direcにいれとく。
90度右を向いて、ポストを左回りで一周回って獲得する。この間右の超音波センサでサーチをしておく。サーチしたデータはtmp変数に入れる。
サーチ自体はほとんどpost_search1と同じものを使い、赤外線コードを監視する関数を別スレッドでたてる。
ただし、restart_flagが１だったら最初に９０度右を向く動作をしない。

void *ir_check_thread(void* post_ID);

post_getから起動されるスレッド。周回中のポストIDを引数にとる。実際はint型だが、(void*)型にキャストして渡している。赤外線コードをtmp_post_codeに記録する。記録、獲得成功で０を返す。見付からない時は１を返す。見付かったが取れなかった場合は２を返す。

右へ90度回っているときはＩＲセンサ0番と2番（これはＭＩＲＳ前方とアーム先端にあるセンサ）を監視し、周回を始めたらＩＲセンサ１番を監視する。

void post_check1(void);

まず最初に、tmp変数内のデータから競技場外のデータを削除する。tmp変数内のデータとテーブル変数内のデータを比較して、同じポストだったらテーブル変数からtmp_post_level と tmp_post_id と tmp_post_got に値を代入する。新しく見つけたポスト(level2)と思われるデータはテーブル変数に追加する。

int post_select2(int post_ID);

次にアプローチするポストを決めて、その方向まで周回し、ポストの方を向く。
引数はサーチ元のポストID。戻り値でアプローチする対象のポストIDを返す。
次ポストの決定方法は、次の優先順位で検索しする。
１、tmp変数の中からlevel2の取ってないポストを探し、その中で一番距離が近いもの
２、tmp変数の中からlevel1の取ってないポストを探し、その中で一番距離が近いもの（ただし来た元のポストのぞく）
３、table変数の中から取ってないポストを探し、その中で一番距離が近いもの（ただし来た元のポストのぞく）
４、table変数の中からすでに取ったポストを探し、その中で一番距離が近いもの（ただし来た元のポストのぞく）
５、今のポストへ来る前のポスト
 

int post_juyu(int now_id);

もう取ったポストへアプローチしたときに、次にアプローチするポストを選択する。

選択基準はpost_select2と同じだが、周回角度が少なくてすむほうの周回方向を選択して、目的のポストの方向まで周回する。

void save_data.c(void);

テーブル変数の内容をファイル（/txt/post_data.txt)に保存する。

ファイルは書き込みモード(w)でオープンし、書き込んだらクローズする。

変数の内容をタブで区切って、postID 0番から一行ずつ記録する。

id ang dis x y level got code ←１行目はこれ。みやすいように

0 数値 数値 数値 数値 数値 数値 数値 ←id=0番のデータ

1 数値 数値 数値 数値 数値 数値 数値 ←id=1番のデータ

これが続いて・・・

数値 数値　←最後の行にはpost_numberとdirec0を記録する。

int accident_check(int target_direc,int uss_restart_flag);

ポスト周回中にpost_getから呼ばれる。ポストに衝突していないか、タッチセンサを監視することで検出する。
衝突を検知したら、いったん下がってアプローチをやり直し、残りの周回角を算出して周回を再開する。
uss_restart_flagが１だったら、ussの連続タスクを再起動する。（サーチ中の場合）
uss_restart_flagが0だったら、ussのタスクは起動しない。（セレクト中の場合）

void illuminate(void);

プログラム終了時に呼ばれる。LEDをカラフルに点滅させる。

void color_set(int r,int g,int b);

MIRS上部のフルカラーLEDを、引数のrgbで指定された色と点灯モードにセットする。

void number_set(int num,int mode);

MIRS上部の５連青色LEDをmodeの点灯モードでnum個点灯させる。

int wait_breath(void);

競技開始前に待機する。このとき、フルカラーＬＥＤを呼吸しているように滑らかに点滅させる。

int correct_direction1(void);

補正角可変システムを導入した。
ＭＩＲＳが首を振り続けているときは補正角を減らし、ＭＩＲＳが同じ方向に何度も向こうとしているときは補正角を増やす。
これにより、補正を平均４回以内で終わらせられる性能を持つ。
ポストへのアプローチ前に（遠くから）補正するため、少しくらいずれていても補正を終える。

int correct_direction2(void);

同じく、補正角可変システムを導入した。

ポストへのアプローチ後に（ポストの近くで）補正する。この角度は周回動作の正確さに大きくかかわるため、精密に補正する。

void read_data.c(void);

ファイル（/txt/post_data.txt)から内容をテーブル変数にロードする。save_data()の書式に合わせて読み込む。ファイルは読み込みモード(r)でオープンし、読み込んだらクローズする。

int post_select3(int now_id);

テーブル変数のデータにより、次にアプローチするポストを決め、そのポストの方を向く。変数keiyuに値を設定する。
引数は今いるポストID（スタート位置の場合は -1）戻り値でアプローチする対象のポストIDを返す。
 
1.次にアプローチするポストを決める（詳しくは後述）
2.そのポストの方を向く。
・スタート位置にいる場合(now_id=-1) ：コンパスを使ってその場回転で対象ポストの方を向く。
・今のkeiyu=0の時（獲得した後） ：そのまま、コンパスを使って目的の方向まで左回り周回する。終わったら右90度回転。
・今のkeiyu=1の時（アプローチしただけ) ：90度左を向いて、コンパスを使って目的の方向まで右回りで周回する。終わったら左90度回転。
 
次ポストの選択方法と、経由フラグの設定
ポスト番号順に取るようにする。
target_codeと同じcodeを持つポストを検索する。なかったら、コード不明（codeが０）のポストの中から、一番近いものを選択する。
コード不明ポストもなければ、そのポスト番号はあきらめてtarget_codeを１増やす。
 
ただし、目標ポストとの間にオジャマポストがあり、それにアプローチした場合は、post_approachで検出され、keiyuが１になっている。
keiyu=0のときはメイン関数でapproachのあとpost_getが呼ばれる。（獲得周回が終わった状態でpost_select3に戻ってくる)
keiyu=1のときはメイン関数でapproachだけして終わり（ポストに正体した状態でpost_select3に戻ってくる)
これによって、次ポストへの周回をどのようにすればよいか決める。
すでに取ったポストだったら周回角が小さくなるように周回方向を選び、まだ取っていないポストだったら獲得してしまわないよう右周りで周回する。

グローバル変数一覧

table変数

ポストのデータを記録する変数群

変数名	概要
int post_x[ ]	ポストのX座標[mm]
int post_y[ ]	ポストのY座標[mm]
int post_level[ ]	ポストのレベル（中心から見つかった=1　ポストから見つかった=2)
int post_ang[ ]	中心から見た各ポストの方位[deg]。コンパスによる値
int post_dis[ ]	中心サーチ時のセンサ値距離データ[mm]
int post_got[ ]	ポストを取ったかどうかのフラグ（取ったら１）
int post_code[ ]	ポストの赤外線コード
int post_number	見つかったポストの数（使っている配列の数）
int post_got_number	取ったポストの数

tmp変数

ポストを周回しながらサーチ(post_get)で、一時的にサーチしたデータをいれとく変数群

変数名	概要
int tmp_post_x[ ]	ポストのX座標[mm]
int tmp_post_y[ ]	ポストのY座標[mm]
int tmp_post_level[ ]	ポストのレベル（中心から見つかった=1　ポストから見つかった=2)
int tmp_post_ang[ ]	tmp_post_direcを基準（０）とするサーチ角
int tmp_post_dis[ ]	ポスト周回中にとったセンサ値距離データ[mm]
int tmp_post_id[ ]	そのtmp変数のデータは、テーブル変数ではどのポストidのものなのかを示す。
int tmp_post_got[ ]	ポストを取ったかどうかのフラグ（取ったら１）
int tmp_post_number	見つかったポストの数（使っている配列の数）
volatile int tmp_post_direc	サーチのためにそのポストに進入した方位の真後ろ（周回サーチが始まる方位）

その他変数

変数名	概要
int target_code	取るべきポストのコード番号。競技二回目で使う
int target_direc	次のポストへアプローチするときの予想方位
int app_post_x;	アプローチ内で一時的に使う変数。アプローチ先を計算するのに使う。post_approachとcircle_position内で使う。
int app_post_y;	同上
int cr_naoki	周回半径[mm]
int last_pos_x	ポスト獲得ループ内で使う変数。ポスト間移動前の基準点（前のポストの位置または競技場の中心位置）をいれておく
int last_pos_y	同上
int last_id	移動前の基準ポストのID　ただし中心だった場合は-1
int keiyu	競技二回目で使う経由フラグ。次にアプローチするポストが獲得対象なのか、ただのオジャマポストなのか判別用
int got_flag	post_get内で起動されるスレッドir_check_threadが設定するフラグ。 0:正常に獲得できた　　-1:IR信号を発見できなかった　　-2:発見したが獲得できなかった
int compass_data	メイン関数とは別のスレッドがコンパスの値を常に更新し続ける変数。
int direc0	競技場の基準方位。二回目では一回目に記録した値を使う。
int renew_compass	コンパスの基準方位を今の測定値に更新するとき１にする。更新されたら０に戻される。スタートボタンが押されたときに１にする。
volatile int led_change_flag	LEDの発光状態を示す下の変数を変更したときに１にする。read_compass_thread内でチェックされ、１ならばled_set実行後に０に戻される。
int led[16]	各LEDの発光状態　 0:消灯　1:点灯　2:blink1,duty1の設定で点滅　3:blink2,duty2の設定で点滅
char blink1[3]	点滅周波数 文字列で指定する。16進二桁　5f とか。3ビット目はNULL
char blink2[3]	同上
char duty1[3]	Duty比。同じく文字列で指定する。
char duty2[3]	同上