/****************************************************************************/
/*	プロジェクト名:	rhand													*/
/*	ファイル名：	sensor.c												*/
/*	内容：			Ｏａｋｓ１６−ＳＥＮＳＯＲ　ＬＡＢＯ					*/
/*						サンプルプログラム　センサ制御部					*/
/*																			*/
/*					　光測距センサを使用出来る様に初期化し、タイマ割込みを	*/
/*					用いて測距を行う。										*/
/*																			*/
/* 特記事項：																*/
/*	日付：			2006.2.6												*/
/*	コンパイラ:		NC30WA (Ver.5.10 Release 1)								*/
/*	note:			OAKS16対応(OAKS16-SENSOR LABO)							*/
/*	作成者：		Kazuhiko Araki											*/
/*																			*/
/****************************************************************************/
/*	 <RCS(CVS)-style>														*/
/*		$Revision: 1.0 $													*/
/*		$Date: 2006/03/27 08:34:58 $										*/
/****************************************************************************/

/* インクルードファイル */
#include	"sfr62a.h"
#include	"sensor.h"						/* センサー制御定義ファイル		*/

/* プロトタイプ宣言 */
void initSensor( void );						/* 光測距センサ初期化関数	*/
int checkSensor( unsigned int threshold );		/* センサ入力判定関数		*/
int checkRightWall( unsigned int r_near, unsigned int r_far );
												/* 右側壁面チェック関数		*/
void SetCpuBoardLED( int sw );					/* CPUボードLED点灯関数		*/
void SetOptionLED( int pat );					/* optionLED点灯関数		*/
static void startAD( int sense_num );			/* AD変換開始関数			*/
static unsigned int getAD( int sense_num );		/* AD変換値取得関数			*/
void ta1int( void );							/* TA1割込み関数			*/

/* 割込み処理の宣言 */
#pragma	INTERRUPT	ta1int	/* ta1int()が割込み処理であることを宣言する。	*/
							/* 指定した関数の入口と出口において、通常の関数	*/
							/* の手続き以外にM16C/60 シリーズ、M16C/20 シリ	*/
							/* ーズの全レジスタの退避・復帰とreit 命令を生	*/
							/* 成します。割り込み処理関数の型は、引数／戻り	*/
							/* 値ともにvoid 型のみ有効。					*/


/* マクロ定義 */
#define	CNT_TA1		(1250-1)	/* timerA1ｶｳﾝﾀ値 fck=f1/8→500ns=5.0e-7[s]	*/
								/* 	timerA0=5.0e-7*1250=0.625[ms]			*/

/* 列挙型変数宣言 */

/* 外部変数宣言 */
unsigned char p0outbuf;			/* ポート０出力バッファ						*/
unsigned char ledoutbuf;		/* ＬＥＤ出力バッファ						*/

unsigned char optledbuf;		/* オプションＬＥＤ出力バッファ				*/

unsigned int addata[END_OF_SENSE];
								/* センサＡＤ変換平均値格納変数				*/

/*--------------------------------------------------------------------------*/
/* 関数名	:initSensor()													*/
/* 機能		:　光測距センサで測距する為に使用するハードウエアリソース		*/
/*			:（資源）を初期化し、測距の準備を行う。							*/
/*			:																*/
/*			:１．使用するポート出力の初期設定を行う。				    	*/
/*			:	P00:	ＣＰＵボード上のＬＥＤは消灯("H"出力)				*/
/*			:	P04:	800Hzパルス出力初期値は"L"							*/
/*			:	P70:	option_led 右センサ感知時点灯						*/
/*			:	P71:	option_led 前センサ感知時点灯						*/
/* 			:	P72:	option_led 左センサ感知時点灯						*/
/*			:	P73:	option_led 右壁適切距離時点灯						*/
/*			:																*/
/*			:２．使用するＡＤ入力の初期設定を行う。					    	*/
/*			:	ADコンバータの設定											*/
/*			:	　１０ビット・サンプル＆ホールド有り						*/
/*			:		ΦＡＤ＝１／６ＭＨｚ（１０ＭＨｚ以下の条件がある。）	*/
/*			:		単発モード使用、ソフトウエアトリガ使用					*/
/*			:　		使用ｃｈ＝０〜２										*/
/*			:			AN0=右センサー										*/
/*			:			AN1=中央センサー									*/
/*			:			AN2=左センサー										*/
/*			:	AD変換時間の計算											*/
/*			:		ΦＡＤ＝１６６．７ｎｓ（１／６ＭＨｚ）					*/
/*			:		１０ビット・サンプル＆ホールド有り時　３３ΦＡＤ		*/
/*			:		変換時間＝３３×１６６．７ｅ−９＝５．５μｓｅｃ		*/
/*			:																*/
/*			:３．使用するタイマの初期設定を行う。							*/
/*			:	TimerA1:	800Hzパルス出力用に初期化する。					*/
/*			:		※TimerA1は、割込みを使用するため、ベクタテーブルに		*/
/*			:		　割込みアドレスの登録をする。(sect30.inc)				*/
/*			:																*/
/*--------------------------------------------------------------------------*/

void initSensor( void )				/* 光測距センサ初期化関数	*/
{
	int i;
	
	/* ポート初期化 		*/
	p0 = 0x01;				/* ポート０レジスタの初期化				*/
								/* bit0:	 1:	ＣＰＵボード上のＬＥＤは消灯("H"出力)	*/
								/* bit1:	 0:	未使用（入力にしておく）				*/
								/* bit2:	 0:	未使用（入力にしておく）				*/
								/* bit3:	 0:	未使用（入力にしておく）				*/
								/* bit4:	 0:	800Hzパルス出力初期値は"L"				*/
								/* bit5:	 0:	未使用（入力にしておく）				*/
								/* bit6:	 0:	未使用（入力にしておく）				*/
								/* bit7:	 0:	未使用（入力にしておく）				*/
	pd0 = 0x11;				/* ポート０方向レジスタの設定初期化		*/
								/* bit0:	 1:	ＣＰＵボード上のＬＥＤ＝出力設定		*/
								/* bit1:	 0:	未使用（入力にしておく）				*/
								/* bit2:	 0:	未使用（入力にしておく）				*/
								/* bit3:	 0:	未使用（入力にしておく）				*/
								/* bit4:	 1:	800Hzパルス出力＝出力設定				*/
								/* bit5:	 0:	未使用（入力にしておく）				*/
								/* bit6:	 0:	未使用（入力にしておく）				*/
								/* bit7:	 0:	未使用（入力にしておく）				*/
	
	/* オプションLED接続ポート */
	p7 = 0x0f;				/* Port7レジスタの設定初期化			*/
								/* bit0:	 1:	option_led 右センサ感知時点灯=消灯		*/
								/* bit1:	 1:	option_led 前センサ感知時点灯=消灯		*/
								/* bit2:	 1:	option_led 左センサ感知時点灯=消灯		*/
								/* bit3:	 1:	option_led 右壁適切距離時点灯=消灯		*/
								/* bit4:	 0:	TA2out（ＰＷＭモードで使用：右モータ）	*/
								/* bit5:	 0:	未使用（入力にしておく）				*/
								/* bit6:	 0:	TA3out（ＰＷＭモードで使用：左モータ）	*/
								/* bit7:	 0:	未使用（入力にしておく）				*/
	pd7 = 0x0f;				/* Port7方向レジスタの設定初期化		*/
								/* bit0:	 1:	option_led 右センサ感知時点灯			*/
								/* bit1:	 1:	option_led 前センサ感知時点灯			*/
								/* bit2:	 1:	option_led 左センサ感知時点灯			*/
								/* bit3:	 1:	option_led 右壁適切距離時点灯			*/
								/* bit4:	 0:	TA2out（ＰＷＭモードで使用：右モータ）	*/
								/* bit5:	 0:	未使用（入力にしておく）				*/
								/* bit6:	 0:	TA3out（ＰＷＭモードで使用：左モータ）	*/
								/* bit7:	 0:	未使用（入力にしておく）				*/

	/* ＡＤ入力設定 */
	adst = 0;				/* AD変換停止							*/
	
	adic = 0x00;			/* AD 割り込みレベルの設定				*/
			 					/* bit2,1,0:   000: 割り込みレベル０（最低:割り込み禁止）*/
								/* bit3: 		 0: 割り込み要求ビット←クリア			*/
	
	adcon2 = 0x01;			/* AD制御レジスタ２の設定				*/
			 					/* bit0:		  1:　サンプル＆ホールド有り			*/
	
	adcon0 = 0x80;			/* AD制御レジスタ０の設定				*/
								/* bit2,1,0:	000:　アナログ入力端子AN0を選択			*/
								/* bit4,3:		 00:　アナログ入力モード＝単発モード	*/
								/* bit5:		  0:　ソフトウエアトリガ選択			*/
								/* bit6:		  0:　ＡＤ変換停止						*/
								/* bit7:		  1:　ＡＤ変換動作周波数fAD/2を選択		*/
	
	adcon1 = 0x28;	 		/* AD制御レジスタ１の設定				*/
								/* bit1,0:		 00:　A-D掃引端子AN0,AN1を選択(単発モード：無効)	*/
								/* bit2:		  0:　A-D動作モード選択繰り返し掃引モード１以外		*/
								/* bit3:		  1:　分解能選択１０ビット選択						*/
								/* bit4:		  0:　ＡＤ変換動作周波数fAD/2またはfAD/4を選択		*/
								/* bit5:		  1:　Vref接続：接続								*/
								/* bit7,6:		 00:　ANEX0,ANEX1は使用しない						*/
	
	/* AD変換の開始はAD入力処理中で行うので初期化処理では停止のまま */

	/* timerA1設定 割り込みを使用する 設定625μs	*/
	ta1ic = 0x04;			/* タイマＡ１割り込みレベルの設定		*/
								/* bit2,1,0: 100: 割り込みレベル４								*/
								/* bit3: 	   0: 割り込み要求ビット←クリア					*/
	ta1mr = 0x40;			/* タイマＡ１タイマモードの設定			*/
								/* bit1,0:	  00:	タイマモード								*/
								/* bit2:	   0:	パルス出力無し								*/
								/* bit4,3:	  0X:	ゲート機能無し（TAiIN端子は通常のﾎﾟｰﾄ端子）	*/
								/* bit5:	   0:	タイマモードでは必ず"0"						*/
								/* bit7,6:	  01:	カウントソース＝ｆ８						*/
	ta1 = CNT_TA1;			/* タイマ値の設定						*/
								/* ｆ８＝周期５００ｎｓｅｃの時									*/
								/* 	(1250-1)を設定すると、1250×500ns=625μs					*/
								/* 	→625μs毎にir_ta1icが”１”になり、ta1int()が呼出される。	*/

	/* 外部変数の初期化	*/
	p0outbuf = 0x00;			/* ポート０出力バッファ						*/
	ledoutbuf = 0x01;			/* ＬＥＤ出力バッファ						*/

	optledbuf = 0x08;			/* オプションＬＥＤ出力バッファ				*/

	for( i = 0; i < END_OF_SENSE; i++ )
	{
		addata[i] = 0;
	}
}

/*----------------------------------------------------------------------*/
/* 関数名	:checkSensor()												*/
/* 引数		:threshold	:センサ入力判定閾値								*/
/* 戻り値	:接近判定値													*/
/*						値0 (=000B) 全センサ反応なし 					*/
/*						値1 (=001B) 右センサだけ反応 					*/
/* 						値2 (=010B) 前センサだけ反応 					*/
/* 						値3 (=011B) 前センサと右センサ反応 				*/
/* 						値4 (=100B) 左センサだけ反応 					*/
/* 						値5 (=101B) 左センサと右センサ反応 				*/
/* 						値6 (=110B) 左センサと前センサ反応 				*/
/* 						値7 (=111B) 全センサ反応 						*/
/* 入力変数（グローバル変数）											*/
/*	unsigned int addata[];			/* センサＡＤ変換値格納変数配列		*/
/* 機能		:既に読み込まれた各センサのＡＤ値を引数で与えられた			*/
/*			 threshold（閾値）と比較しＡＤ値の方が大きければ戻り地の	*/
/*			 対応するビットを立てる。									*/
/*			 このとき接近を示すＣＰＵボード上のＬＥＤを接近していれば	*/
/*			 点灯、そうでなければ消灯する。							    */
/*----------------------------------------------------------------------*/
int checkSensor( int threshold )
{
	int result;						/* 判定結果格納用変数				*/
	int sw;							/* ＣＰＵボード上ＬＥＤ点灯用変数	*/
	int r,c,l;
	
	if(threshold > 1023 || threshold<0 ) //10bit 以上にはならないので，センサを使用しないものとみなす
		return 0;   // センサに反応なし，LEDも変更せず

	r = S_RIGHT;
	c = S_CENTER;
	l = S_LEFT;
	
	/* ＡＤ変換値の判定 */
	result = 0;						/* 判定結果の初期化			*/
	if( addata[S_RIGHT]  > threshold )
	{
		result |= 0x01;				/* 右センサが閾値以上の時ビット０を立てる */
	}
	
	if( addata[S_CENTER]  > threshold )
	{
		result |= 0x02;				/* 前センサが閾値以上の時ビット１を立てる */
	}
	
	if( addata[S_LEFT]  > threshold )
	{
		result |= 0x04;				/* 左センサが閾値以上の時ビット２を立てる */
	}
	
	/* オプションＬＥＤ点灯 */
	SetOptionLED( result );			/* オプションＬＥＤ点消灯設定		*/
	
	return result;
}

/*----------------------------------------------------------------------*/
/* 関数名	:checkRightWall()											*/
/* 引数		:無し														*/
/* 戻り値	:接近判定値													*/
/*						値0 右壁距離範囲内	 					*/
/*						値1 右壁距離接近 						*/
/* 						値2 右壁反応なし						*/
/* 						値-1 エラー						*/
/* 入力変数（グローバル変数）											*/
/*	unsigned int right_sensor;		/* 右センサＡＤ変換値格納変数		*/
/* 機能		:既に読み込まれた右センサのＡＤ値と定数で与えられた閾値を	*/
/*			 比較しＡＤ値が上記範囲で判定する。							*/
/*----------------------------------------------------------------------*/
int checkRightWall( int r_near, int r_far )
{
	int result;						/* 判定結果格納用変数					*/

	if(r_near <= r_far)
		result = -1;
	else {
		if(r_near>1023)
			r_near=1023;
		if(r_far<0)
			r_far = 0;
	}

	/* ＡＤ変換値の判定 */
	result = 0;						/* 判定結果の初期化						*/
	if( addata[S_RIGHT] > r_near )
	{
		result |= 0x01;				/* 右センサが閾値以上の時ビット０を立てる */
	}
	if( addata[S_RIGHT] < r_far )
	{
		result |= 0x02;				/* 右センサが閾値以上の時ビット０を立てる */
	}
	
	/* 右壁距離判定結果をオプションＬＥＤbit3に反映 */
	if( result == 0 )
	{
		optledbuf = 0x00;
	}
	else
	{
		optledbuf = 0x08;
	}
	
	return result;
}

/*----------------------------------------------------------------------*/
/* 関数名	:readSensor()												*/
/* 引数		:sensor_no	:センサ番号
/*				　値0 右センサ								*/
/*				　値1 前センサ								*/
/*				　値2 左センサ								*/
/* 戻り値	:センサ値(距離に相当)	*/												
/* 入力変数（グローバル変数）											*/
/*	unsigned int addata[];			/* センサＡＤ変換値格納変数配列		*/
/* 機能		:引数で与えられたセンサの，既に読み込まれたＡＤ値を返す			*/
/*----------------------------------------------------------------------*/
int readSensor( int sensor_no )
{
	if(sensor_no<0||sensor_no>2)
		return 0;
        else
		return adddata[sensor_no];
}

/*----------------------------------------------------------------------*/
/* 関数名	:SetCpuBoardLED()											*/
/* 引数		:sw		:ＣＰＵボード上ＬＥＤ点灯・消灯スイッチ				*/
/* 戻り値	:無し														*/
/* 機能		:引数で指定された状態でＬＥＤを点消灯させる					*/
/*----------------------------------------------------------------------*/

void SetCpuBoardLED( int sw )
{
	/* P00LED（ＣＰＵボード上のＬＥＤ）点灯・消灯設定 */
	if( sw == OFF )
	{
		ledoutbuf = 0x01;
	}
	else
	{
		ledoutbuf = 0x00;
	}
}

/*----------------------------------------------------------------------*/
/* 関数名	:SetOptionLED()												*/
/* 引数		:pat	:オプションＬＥＤ点灯パターン						*/
/* 戻り値	:無し														*/
/* 機能		:引数で指定されたパターンでＬＥＤを点消灯させる				*/
/*----------------------------------------------------------------------*/

void SetOptionLED( int pat )
{
	const unsigned char pattern[] = {	0x07,	/* 0	*/
										0x03,	/* 1	*/
										0x05,	/* 2	*/
										0x01,	/* 3	*/
										0x06,	/* 4	*/
										0x02,	/* 5	*/
										0x04,	/* 6	*/
										0x00	/* 7	*/
									};
	
	/* P70〜P73のLED（オプションのＬＥＤ）点灯・消灯設定 */
	p7 = pattern[ pat & 0x07] | optledbuf;
}

/*--------------------------------------------------------------*/
/* 関数名	:startAD()											*/
/* 機能		:センサ用ＡＤ変換開始関数						    */
/*　		:引数で与えられた数値に対応するセンサのＡＤ変換を	*/
/*			:開始する。											*/
/* 引数 	: sense_num	:	センサ選択値						*/
/* 戻り値	: 			:	無し								*/
/*--------------------------------------------------------------*/
static void startAD( int sense_num )
{
	if( sense_num < END_OF_SENSE )
	{
		adst = 0;						/* ＡＤ変換停止						*/
		adcon0 = 0x80 | sense_num;		/* AD制御レジスタ０で変換するＡＤチャネルを指定	*/
		
		adst = 1;						/* ＡＤ変換開始						*/
	}
	
	return;
}

/*--------------------------------------------------------------*/
/* 関数名	:getAD()											*/
/* 機能		:センサ用ＡＤ変換値取得関数						    */
/*　		:引数で与えられた数値に対応するセンサのＡＤ変換値を	*/
/*			:読み込む。											*/
/* 引数 	: sense_num	:	センサ選択値						*/
/* 戻り値	: 			:	AD変換値							*/
/*--------------------------------------------------------------*/
static unsigned int getAD( int sense_num )
{
	unsigned int result;			/* 戻り値を格納する変数。			*/
	
	result = 0xffff;	/* 該当外の引数が選ばれた時や変換が完了していな	*/
						/* かった時のためのダミーの値を設定する。		*/
	
	if( ir_adic == 1 )				/* ＡＤ変換完了？					*/
	{						/* yes */
		ir_adic = 0;				/* ＡＤ変換完了割り込み要求ビットクリア	*/
		
		adst = 0;					/* ＡＤ変換停止						*/
		
		switch( sense_num )
		{
			case S_RIGHT:
				result = ad0;		/* 右センサＡＤ変換データ読み込み	*/
				break;
			case S_CENTER:
				result = ad1;		/* 中央センサＡＤ変換データ読み込み	*/
				break;
			case S_LEFT:
				result = ad2;		/* 左センサＡＤ変換データ読み込み	*/
				break;
			default:
				break;
		}
		
		result &= 0x3ff;			/* 変換値は10ビットなのでごみが無い様にマスクする。 */
	}
	
	return result;
}

/*--------------------------------------------------------------*/
/* 関数名	:ta1int()											*/
/* 機能		:ta1割り込み関数								    */
/*　		:割り込みのたびに距離センサ用８００Ｈｚのパルス出力	*/
/*			:０．６２５ミリ秒計時（タイマA1）					*/
/* 			:	タイマA1のカウントソースをf8とする				*/
/* 			:	1カウントは1/(16MHz/8)=０．５μS				*/
/* 			:	625μsを計時するには625μs/0.5μs=1250カウント	*/
/*--------------------------------------------------------------*/

void ta1int( void )
{
	static int adcycle = 0x0f;
	static unsigned int adbuffer[ END_OF_SENSE ] = { 0, 0, 0 };
	static unsigned int adcount[ END_OF_SENSE ] = { 0, 0, 0 };
	int sense_num;
	unsigned int result;
	int i;
	
	/* 800Hzパルス出力 */
	p0outbuf = p0outbuf ^ 0x10;			/* ﾎﾟｰﾄ0のbit4:   パルス出力ポートビット位置	*/
	p0outbuf = p0outbuf & 0xfe;
	p0outbuf = p0outbuf | ledoutbuf;
	p0 = p0outbuf;
	
	/* センサＡＤ変換値の取得 */
	sense_num = adcycle & 0x03;
	if( sense_num < 0x03 )
	{
		{
			result = getAD( sense_num );
			if( result != 0xffff )
			{
				adbuffer[ sense_num ] += result;	/* 平均算出用加算	*/
				adcount[ sense_num ]++;				/* 加算回数カウント */
			}
		}
	}
	else
	{
		if( adcycle == 0x0f )
		{
			for( i = 0; i < END_OF_SENSE; i++ )
			{
				if( adcount[ i ] != 0 )
				{
					addata[ i ] = adbuffer[ i ] / adcount[ i ];
													/* センサＡＤ変換平均値計算	*/
					adcount[ i ] = 0;
				}
				
				adbuffer[ i ] = 0;
			}
		}
	}
	
	/* ＡＤ読込サイクル更新 */
	adcycle++;
	adcycle &= 0x0f;
	
	/* センサＡＤ入力開始 */
	if( adcycle != 0x03 )
	{
		startAD( adcycle & 0x03 );					/* ＡＤ変換開始			*/
	}
}