/*
 *	RC PROPO の 4ch PWM信号を TMP0-3 にてキャプチャし、
 *	TMQ00-03 の端子から、その合成 PWM 信号を出力する。
 *
 *		使用している CPU, ボードは、
 *		NEC Electronics 製 V850ES/JG2, CQ出版社Interface誌2007年5月号付録
 *		（同仕様のボードが ESP企画にて発売中 http://www.esp.co.jp）
 *
 *	注意１）
 *			本プログラムは動作確認をしていません。
 *			利用に際してなにがしかのトラブルが発生しても当方は責任を負いません。
 *
 *	注意２）
 *			RC PROPO の ch0-3 がどのスティックに割り振られているかは、
 *			ユーザの皆さんで確認の上、利用してください。
 *			また、ニュートラルな信号は 1.5msec.の PWM としています。
 *			合わせて変更が必要な場合は行ってください。
 *
 *	接続）
 *			RC PROPO CH0	<->		P32 + P33
 *			RC PROPO CH1	<->		P34 + P35
 *			RC PROPO CH2	<->		P96 + P97
 *			RC PROPO CH3	<->		P94 + P95
 *
 *			出力サーボ１	<->		P53
 *			出力サーボ２	<->		P50
 *			出力サーボ３	<->		P51
 *			出力サーボ４	<->		P52
 *
 *			もし、出力サーボ1-4 の PWM 信号の電圧が 5V 程度必要であれば、
 *			P50-53 を NchOpenDrain モードに設定し、10k〜100k程度の抵抗にて
 *			+5V にてプルアップしてください。
 *
 *	補足）
 *			ソフトウェア記述を簡略化するため、RC PROPO とマイコンの接続を
 *				CH0 <-> TIP00 +  TIP01
 *				CH1 <-> TIP10 +  TIP11
 *				CH2 <-> TIP20 +  TIP21
 *				CH3 <-> TIP30 +  TIP31
 *			としています。　TIP?0 を立ち上がりエッヂの割込み、TIP?1 を立ち下がり
 *			エッヂの割込みに設定し、その時間差をPWM長としています。
 *			TIP?0 の割込み発生時に TP?CC0 カウンタが 0 になるため、TIP?1 の割込み
 *			発生時に同カウンタを読むだけで時間差、つまりPWM長となります。
 *
 *			TAB CODE は 4 スペースです。
 *
 *		#2009.3.7	Norio OKADA
 *
 */
#pragma	ioreg

#pragma interrupt INTTQ0CC0	intc_TMQ0CC0
#pragma interrupt INTTP0CC1	intc_TMP0CC1
#pragma interrupt INTTP1CC1	intc_TMP1CC1
#pragma interrupt INTTP2CC1	intc_TMP2CC1
#pragma interrupt INTTP3CC1	intc_TMP3CC1

#define	NEUTRAL		(1500*20/8)
//
//		グローバル変数定義
//
short	capture_ch0;		// たとえば右スティックの上下値とする
short	capture_ch1;		// たとえば右スティックの左右値とする
short	capture_ch2;		// たとえば左スティックの上下値とする
short	capture_ch3;		// たとえば左スティックの左右値とする
//
//		割り込み処理ルーチン（TQ0,TP0-3）
//
__interrupt
void		intc_TMQ0CC0( void )
{
	TQ0CCR0	= capture_ch0;
	TQ0CCR1	= capture_ch0;
	TQ0CCR2	= capture_ch0;
	TQ0CCR3	= capture_ch0;
	if( capture_ch1 > NEUTRAL ) {
		TQ0CCR0	+= (capture_ch1 - NEUTRAL);
		TQ0CCR1	+= (capture_ch1 - NEUTRAL);
	}
	else {
		TQ0CCR2	+= (NEUTRAL - capture_ch1);
		TQ0CCR3	+= (NEUTRAL - capture_ch1);
	}
	if( capture_ch2 > NEUTRAL ) {
		TQ0CCR0	+= (capture_ch2 + NEUTRAL);
	}
	else {
		TQ0CCR1	+= (NEUTRAL - capture_ch2);
	}
	if( capture_ch3 > NEUTRAL ) {
		TQ0CCR2	+= (capture_ch3 + NEUTRAL);
	}
	else {
		TQ0CCR3	+= (NEUTRAL - capture_ch3);
	}
}
__interrupt
void		intc_TMP0CC1( void )
{
	capture_ch0 = TP0CCR1;
}
__interrupt
void		intc_TMP1CC1( void )
{
	capture_ch1 = TP1CCR1;
}
__interrupt
void		intc_TMP2CC1( void )
{
	capture_ch2 = TP2CCR1;
}
__interrupt
void		intc_TMP3CC1( void )
{
	capture_ch3 = TP3CCR1;
}
//
//		TMQ0 処理：ＰＷＭ出力モード
//
void		init_TMQ0( void )
{
	TQ0CE		= 0;						/* TQ0CTL0.7：TMQ0動作禁止						*/
	TQ0OVIC		= 0x47;						/* INTTQOV割込み許可＆優先レベル：7				*/
	TQ0CTL0		= 0x03;						/* 内部カウント設定：fxx/8 (Cycle=32.768msec)	*/
	TQ0CTL1		= 0b00000101;				/* フリーランニングモード						*/
	TQ0IOC0		= 0b01010101;				/* TOQ00-03出力許可, TOQ00-03=LowActive			*/
	TQ0CCR0		= NEUTRAL;					/* TQ00パルス幅設定	 1,500[usec] * 20MHz / 8	*/
	TQ0CCR1		= NEUTRAL;					/* TQ01パルス幅設定	 1,500[usec] * 20MHz / 8	*/
	TQ0CCR2		= NEUTRAL;					/* TQ02パルス幅設定	 1,500[usec] * 20MHz / 8	*/
	TQ0CCR3		= NEUTRAL;					/* TQ03パルス幅設定	 1,500[usec] * 20MHz / 8	*/
}
//
//		TMP0 処理：パルス幅測定モード
//
void		init_TMP0( void )
{
	TP0CE		= 0;						/* TP0CTL0.7：TMP0動作禁止						*/
	TP0OVIC		= 0x47;						/* INTTP0OV割込み許可＆優先レベル：7			*/
	TP0CTL0		= 0x03;						/* 内部カウント設定：fxx/8 (Cycle=32.768msec)	*/
	TP0CTL1		= 0b00000110;				/* パルス幅測定モード							*/
	TP0OPT0		= 0b00110000;				/* TP0CCS0,1 をキャプチャレジスタに選択			*/
	TP0IOC1		= 0b00001001;				/* TIP00 立上りエッヂ, TIP01 立下りエッチ		*/
}
//
//		TMP1 処理：パルス幅測定モード
//
void		init_TMP1( void )
{
	TP1CE		= 0;						/* TP1CTL0.7：TMP1動作禁止						*/
	TP1OVIC		= 0x47;						/* INTTP1OV割込み許可＆優先レベル：7			*/
	TP1CTL0		= 0x03;						/* 内部カウント設定：fxx/8 (Cycle=32.768msec)	*/
	TP1CTL1		= 0b00000110;				/* パルス幅測定モード							*/
	TP1OPT0		= 0b00110000;				/* TP1CCS0,1 をキャプチャレジスタに選択			*/
	TP1IOC1		= 0b00001001;				/* TIP10 立上りエッヂ, TIP11 立下りエッチ		*/
}
//
//		TMP2 処理：パルス幅測定モード
//
void		init_TMP2( void )
{
	TP2CE		= 0;						/* TP2CTL0.7：TMP2動作禁止						*/
	TP2OVIC		= 0x47;						/* INTTP2OV割込み許可＆優先レベル：7			*/
	TP2CTL0		= 0x03;						/* 内部カウント設定：fxx/8 (Cycle=32.768msec)	*/
	TP2CTL1		= 0b00000110;				/* パルス幅測定モード							*/
	TP2OPT0		= 0b00110000;				/* TP2CCS0,1 をキャプチャレジスタに選択			*/
	TP2IOC1		= 0b00001001;				/* TIP20 立上りエッヂ, TIP21 立下りエッチ		*/
}
//
//		TMP3 処理：パルス幅測定モード
//
void		init_TMP3( void )
{
	TP3CE		= 0;						/* TP3CTL0.7：TMP3動作禁止						*/
	TP3OVIC		= 0x47;						/* INTTP3OV割込み許可＆優先レベル：7			*/
	TP3CTL0		= 0x03;						/* 内部カウント設定：fxx/8 (Cycle=32.768msec)	*/
	TP3CTL1		= 0b00000110;				/* パルス幅測定モード							*/
	TP3OPT0		= 0b00110000;				/* TP3CCS0,1 をキャプチャレジスタに選択			*/
	TP3IOC1		= 0b00001001;				/* TIP30 立上りエッヂ, TIP31 立下りエッチ		*/
}
//
//		CPU initialization
//
int		init_CPU( void )
{
	extern unsigned long	_S_romp;

	VSWC	= 1;							/* システムウェイトコントロールレジスタ			*/
	WDTM2	= 0;							/* WDTM動作禁止									*/
	__asm( "mov		0x00, r11" );
	__asm( "st.b	r11, PRCMD" );			/* 特定レジスタを使用する前にPMCMDに書き込む	*/
	__asm( "st.b	r11, PCC" );			/* PCCの設定 PCC = 00:							*/
	while( LOCK );							/* 1:UnLock，0:Lock -> 周波数安定待ち			*/
	SELPLL	= 1;							/* 1:PLL動作 0:PLL停止							*/
	return _rcopy(&_S_romp, -1);			/* ROM化処理									*/
}
//
//		PORT initialization
//
void		init_PORT( void )
{
	TQ0CE		= 0;						/* TQ0CTL0.7：TMQ0動作禁止						*/
	TP0CE		= 0;						/* TP0CTL0.7：TMP0動作禁止						*/
	TP1CE		= 0;						/* TP1CTL0.7：TMP1動作禁止						*/
	TP2CE		= 0;						/* TP2CTL0.7：TMP2動作禁止						*/
	TP3CE		= 0;						/* TP3CTL0.7：TMP3動作禁止						*/

#if 0	// +5Vの出力が欲しい場合 0 -> 1 に変更し、回路上でプルアップ抵抗を入れてください
	PF5.0		= 1;						/* P50 : NchOpenDrain モード設定				*/
	PF5.1		= 1;						/* P51 : NchOpenDrain モード設定				*/
	PF5.2		= 1;						/* P52 : NchOpenDrain モード設定				*/
	PF5.3		= 1;						/* P53 : NchOpenDrain モード設定				*/
#endif

	// ポートモードレジスタ（入出力の選択 0=出力, 1=入力（デフォルト））
	PM5.0		= 0;						/* P50 : TOQ01 ポートモード出力設定				*/
	PM5.1		= 0;						/* P51 : TOQ02 ポートモード出力設定				*/
	PM5.2		= 0;						/* P52 : TOQ03 ポートモード出力設定				*/
	PM5.3		= 0;						/* P53 : TOQ00 ポートモード出力設定				*/

	PM3L.2		= 1;						/* P32 : TOP00 ポート入力設定					*/
	PM3L.3		= 1;						/* P33 : TOP01 ポート入力設定					*/
	PM3L.4		= 1;						/* P34 : TOP10 ポート入力設定					*/
	PM3L.5		= 1;						/* P35 : TOP11 ポート入力設定					*/

	PM9L.4		= 1;						/* P94 : TOP31 ポート入力設定					*/
	PM9L.5		= 1;						/* P95 : TOP30 ポート入力設定					*/
	PM9L.6		= 1;						/* P96 : TOP21 ポート入力設定					*/
	PM9L.7		= 1;						/* P97 : TOP20 ポート入力設定					*/

	// ポートファンクションコントロール拡張レジスタ（拡張兼用機能選択）
	PFCE5.0		= 1;						/* P50 : TOQ01/RTP00 端子設定					*/
	PFCE5.1		= 1;						/* P51 : TOQ02/RTP01 端子設定					*/
	PFCE5.2		= 1;						/* P52 : TOQ03/RTP02 端子設定					*/
	PFCE5.3		= 1;						/* P53 : TOQ00/RTP03 端子設定					*/

	PFCE9L.4	= 0;						/* P94 : TIP31 端子設定							*/
	PFCE9L.5	= 0;						/* P95 : TIP30 端子設定							*/
	PFCE9L.6	= 1;						/* P96 : TIP21 端子設定							*/
	PFCE9L.7	= 1;						/* P97 : TIP20 端子設定							*/

	PFCE3L.2	= 1;						/* P32 : TIP00 端子設定							*/

	// ポートファンクションコントロールレジスタ（兼用機能選択）
	PFC5.3		= 0;						/* P53 : TOQ00 端子設定			PFCE5.3 も必要	*/
	PFC5.0		= 0;						/* P50 : TOQ01 端子設定			PFCE5.0 も必要	*/
	PFC5.1		= 0;						/* P51 : TOQ02 端子設定			PFCE5.1 も必要	*/
	PFC5.2		= 0;						/* P52 : TOQ03 端子設定			PFCE5.2 も必要	*/

	PFC3L.2		= 0;						/* P32 : TIP00 端子設定			PFCE3L.2も必要	*/
	PFC3L.3		= 0;						/* P33 : TIP01 端子設定							*/
	PFC3L.4		= 0;						/* P34 : TIP10 端子設定							*/
	PFC3L.5		= 0;						/* P35 : TIP11 端子設定							*/

	PFC9L.4		= 1;						/* P94 : TIP31 端子設定			PFCE9L.4も必要	*/
	PFC9L.5		= 1;						/* P95 : TIP31 端子設定			PFCE9L.5も必要	*/
	PFC9L.6		= 0;						/* P96 : TIP21 端子設定			PFCE9L.6も必要	*/
	PFC9L.7		= 0;						/* P96 : TIP20 端子設定			PFCE9L.7も必要	*/

	// ポートモードコントロールレジスタ（0=汎用（デフォルト）, 1=兼用機能）
	PMC5.0		= 1;						/* P50 : TOQ01/TIQ01 端子設定					*/
	PMC5.1		= 1;						/* P51 : TOQ02/TIQ02 端子設定					*/
	PMC5.2		= 1;						/* P52 : TOQ03/TIQ03 端子設定					*/
	PMC5.3		= 1;						/* P53 : TOQ00/TIQ00 端子設定					*/

	PMC3L.2		= 1;						/* P32 : TOP00/TIP00 端子設定					*/
	PMC3L.3		= 1;						/* P33 : TOP01/TIP01 端子設定					*/
	PMC3L.4		= 1;						/* P34 : TOP10/TIP10 端子設定					*/
	PMC3L.5		= 1;						/* P35 : TOP11/TIP11 端子設定					*/

	PMC9L.4		= 1;						/* P94 : TOP31/TIP31 端子設定					*/
	PMC9L.5		= 1;						/* P95 : TOP30/TIP30 端子設定					*/
	PMC9L.6		= 1;						/* P96 : TOP21/TIP21 端子設定					*/
	PMC9L.7		= 1;						/* P97 : TOP20/TIP20 端子設定					*/

	PCT.6		= 1;						/* LED1用 消灯 (LowActive)						*/
	PMCT.6		= 0;						/* LED1 出力ポートに設定						*/
}
//
//		INTC initialization
//
void		init_INTC( void )
{
	init_TMQ0();							/* TMQ0 initialize								*/
	init_TMP0();							/* TMP0 initialize								*/
	init_TMP1();							/* TMP1 initialize								*/
	init_TMP2();							/* TMP2 initialize								*/
	init_TMP3();							/* TMP3 initialize								*/

	__EI();									/* 割り込み許可									*/

	TQ0CE	= 1;							/* TMQ0 動作許可								*/
	TP0CE	= 1;							/* TMP0 動作許可								*/
	TP1CE	= 1;							/* TMP1 動作許可								*/
	TP2CE	= 1;							/* TMP2 動作許可								*/
	TP3CE	= 1;							/* TMP3 動作許可								*/

	TQ0CCMK0	= 0;						/* INTTQ0CC0割込み許可							*/

	TP0CCMK1	= 0;						/* INTTP0CC1割込み許可							*/
	TP1CCMK1	= 0;						/* INTTP1CC1割込み許可							*/
	TP2CCMK1	= 0;						/* INTTP2CC1割込み許可							*/
	TP3CCMK1	= 0;						/* INTTP3CC1割込み許可							*/
}
int		main( void )
{
	init_CPU();								/* CPU  initialize								*/
	init_PORT();							/* PORT initialize								*/
	init_INTC();							/* INTC initialize								*/
	while( 1 ) {
		//　何もしていない。　割込みだけの処理に任せている。
	}
	return 0;
}