#include "iodefine.h"
#include "intMTU.h"

//MTU21,MTU22は位相計測と被るので併用不可

volatile short _pwm_duty[16]={0}; // 各ポート用デューティー比設定

// ServoPulsの初期化
void init_MTU(void){
	PWM_IO_init();
	MTU2_init();	
	pwm_duty_init(); // ServoPuls初期化
	MTU2_syn_start(); // タイマシンクロスタート
}

void pwm_duty_init(void)
{
	int i;
	for (i = 0; i < 16; i++) {
		_pwm_duty[i] = 0; //PWM出力を全て0に設定
	}
}

// IO Portのセット
void PWM_IO_init(void){

	// ------------------------------------------------------------------
	// PortE サーボパルス用　[PE15:PE0] MTU2
	// ------------------------------------------------------------------
	PFC.PECRL4.BIT.PE15MD = 0x01; // PE15 is TIOC4D
	PFC.PECRL4.BIT.PE14MD = 0x01; // PE14 is TIOC4C
	PFC.PECRL4.BIT.PE13MD = 0x01; // PE13 is TIOC4B
	PFC.PECRL4.BIT.PE12MD = 0x01; // PE12 is TIOC4A
/**/
	PFC.PECRL3.BIT.PE11MD = 0x01; // PE11 is TIOC3D
	PFC.PECRL3.BIT.PE10MD = 0x01; // PE10 is TIOC3C
	PFC.PECRL3.BIT.PE9MD = 0x01; // PE9 is TIOC3B
	PFC.PECRL3.BIT.PE8MD = 0x01; // PE8 is TIOC3A
/*
	PFC.PECRL2.BIT.PE7MD = 0x01; // PE7 is TIOC2B
	PFC.PECRL2.BIT.PE6MD = 0x01; // PE6 is TIOC2A
	PFC.PECRL2.BIT.PE5MD = 0x01; // PE5 is TIOC1B
	PFC.PECRL2.BIT.PE4MD = 0x01; // PE4 is TIOC1A
/**/
//	PFC.PECRL1.BIT.PE3MD = 0x01; // PE3 is TIOC0D
//	PFC.PECRL1.BIT.PE2MD = 0x01; // PE2 is TIOC0C
	PFC.PECRL1.BIT.PE3MD = 0x00; // PE3 is I/O
	PFC.PECRL1.BIT.PE2MD = 0x00; // PE2 is I/O
	PFC.PECRL1.BIT.PE1MD = 0x01; // PE1 is TIOC0B
	PFC.PECRL1.BIT.PE0MD = 0x01; // PE0 is TIOC0A

//	PFC.PEIORL.WORD = 0xFFFF; // PE15-PE0 is output
	PFC.PEIORL.WORD = 0xFFF3; // PE3-PE2 is input other output
}

// MTU2の初期化
void MTU2_init(void){
	STB.CR4.BIT._MTU2 = 0; // モジュールスタンバイの解除
	MTU2_syn_stop(); 		// タイマ0,3,4停止
	MTU2.TRWER.BIT.RWE = 1; // 3,4のレジスタのリードライトを許可する
//	MTU2.TSYR.BYTE = 0xC7; // TCNT0〜4同期動作
	MTU2.TSYR.BYTE = 0xC1; // TCNT0,3,4同期動作
	
	// タイマ0〜4同期動作
	MTU20.TCR.BYTE = 0x63; // 同期クリア、MPφ/64 1count->2.56e-6s 1ms->390.6count
//	MTU21.TCR.BYTE = 0x63; // 同期クリア、MPφ/64
//	MTU22.TCR.BYTE = 0x63; // 同期クリア、MPφ/64
	MTU23.TCR.BYTE = 0x63; // 同期クリア、MPφ/64
	MTU24.TCR.BYTE = 0x63; // 同期クリア、MPφ/64
	
	MTU20.TMDR.BYTE = 0x00; // タイマ通常動作
//	MTU21.TMDR.BYTE = 0x00;
//	MTU22.TMDR.BYTE = 0x00;
	MTU23.TMDR.BYTE = 0x00;
	MTU24.TMDR.BYTE = 0x00;
	
	MTU20.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
//	MTU21.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
//	MTU22.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
//	MTU23.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
//	MTU24.TIER.BIT.TCIEV = 1; // OVFによる割り込み許可
	
	INTC.IPRD.BIT._MTU20C = 15; // MTU20 OVF 割り込み優先度 15
	
	MTU2.TOER.BYTE = 0xFF; // 3,4の出力許可
	
	MTU20.TIOR.BIT.IOA = 0x01; // TGRA_0 PE0 出力停止
	MTU20.TIOR.BIT.IOB = 0x01; // TGRB_0 PE1 出力停止
//	MTU20.TIOR.BIT.IOC = 0x01; // TGRC_0 PE2 出力停止
//	MTU20.TIOR.BIT.IOD = 0x01; // TGRD_0 PE3 出力停止
/*
	MTU21.TIOR.BIT.IOA = 0x01; // TGRA_1 PE4 出力停止
	MTU21.TIOR.BIT.IOB = 0x01; // TGRB_1 PE5 出力停止
	MTU22.TIOR.BIT.IOA = 0x01; // TGRA_2 PE6 出力停止
	MTU22.TIOR.BIT.IOB = 0x01; // TGRB_2 PE7 出力停止
*/
	MTU23.TIOR.BIT.IOA = 0x01; // TGRA_3 PE8 出力停止
	MTU23.TIOR.BIT.IOB = 0x01; // TGRB_3 PE9 出力停止
	MTU23.TIOR.BIT.IOC = 0x01; // TGRC_3 PE10 出力停止
	MTU23.TIOR.BIT.IOD = 0x01; // TGRD_3 PE11 出力停止
	MTU24.TIOR.BIT.IOA = 0x01; // TGRA_4 PE12 出力停止
	MTU24.TIOR.BIT.IOB = 0x01; // TGRB_4 PE13 出力停止
	MTU24.TIOR.BIT.IOC = 0x01; // TGRC_4 PE14 出力停止
	MTU24.TIOR.BIT.IOD = 0x01; // TGRD_4 PE15 出力停止
	
	MTU20.TGRA = PWM_DEFAULT+0; // 比較用設定値
	MTU20.TGRB = PWM_DEFAULT+0;
//	MTU20.TGRC = PWM_DEFAULT+0;
//	MTU20.TGRD = PWM_DEFAULT+0;
/*
	MTU21.TGRA = PWM_DEFAULT+0;
	MTU21.TGRB = PWM_DEFAULT+0;
	MTU22.TGRA = PWM_DEFAULT+0;
	MTU22.TGRB = PWM_DEFAULT+0;
*/
	MTU23.TGRA = PWM_DEFAULT+0; // 比較用設定値
	MTU23.TGRB = PWM_DEFAULT+0;
	MTU23.TGRC = PWM_DEFAULT+0;
	MTU23.TGRD = PWM_DEFAULT+0;
	MTU24.TGRA = PWM_DEFAULT+0;
	MTU24.TGRB = PWM_DEFAULT+0;
	MTU24.TGRC = PWM_DEFAULT+0;
	MTU24.TGRD = PWM_DEFAULT+0;
	
	MTU20.TCNT = 0x0;
	
	MTU20.TSR.BIT.TCFV = 0; // OVFフラグクリア
	
}

//****************************************************************************
//	関数名	:MTU20_INT_OVF
//	内容	:MTU20 OVF 割り込み関数
//
//			この関数は、HEWのintprg.cで定義しておく必要がある
//			※TCNT_0オーバフローの割り込み発生時に、HEWのINT_MTU2_0_TCIV0関数からコールされる 
//****************************************************************************

void MTU20_INT_OVF(void){
	MTU2_syn_stop();		// タイマ0〜4停止
	MTU20.TSR.BIT.TCFV = 0;		// OVFフラグクリア
	MTU20.TCNT = PWM_DEFAULT;	// タイマーセット

	if(_pwm_duty[0]<=0)		MTU20.TIOR.BIT.IOA = 0x01;	// TGRA_0 PE0 OFF→OFF
	else					MTU20.TIOR.BIT.IOA = 0x05;	// TGRA_0 PE0 ON→OFF

	if(_pwm_duty[1]<=0)		MTU20.TIOR.BIT.IOB = 0x01;	// TGRB_0 PE1 OFF→OFF
	else					MTU20.TIOR.BIT.IOB = 0x05;	// TGRB_0 PE1 ON→OFF

//	if(_pwm_duty[2]<=0)		MTU20.TIOR.BIT.IOC = 0x01;	// TGRC_0 PE2 OFF→OFF
//	else					MTU20.TIOR.BIT.IOC = 0x05;	// TGRC_0 PE2 ON→OFF

//	if(_pwm_duty[3]<=0)		MTU20.TIOR.BIT.IOD = 0x01;	// TGRD_0 PE3 OFF→OFF
//	else					MTU20.TIOR.BIT.IOD = 0x05;	// TGRD_0 PE3 ON→OFF

/**//*
	if(_pwm_duty[4]<=0)		MTU21.TIOR.BIT.IOA = 0x01;	// TGRA_1 PE4 OFF→OFF
	else					MTU21.TIOR.BIT.IOA = 0x05;	// TGRA_1 PE4 ON→OFF

	if(_pwm_duty[5]<=0)		MTU21.TIOR.BIT.IOB = 0x01;	// TGRB_1 PE5 OFF→OFF
	else					MTU21.TIOR.BIT.IOB = 0x05;	// TGRB_1 PE5 ON→OFF
/**//*
	if(_pwm_duty[6]<=0)		MTU22.TIOR.BIT.IOA = 0x01;	// TGRA_2 PE6 OFF→OFF
	else					MTU22.TIOR.BIT.IOA = 0x05;	// TGRA_2 PE6 ON→OFF

	if(_pwm_duty[7]<=0)		MTU22.TIOR.BIT.IOB = 0x01;	// TGRB_2 PE7 OFF→OFF
	else					MTU22.TIOR.BIT.IOB = 0x05;	// TGRB_2 PE7 ON→OFF
/**/

	if(_pwm_duty[8]<=0)		MTU23.TIOR.BIT.IOA = 0x01;	// TGRA_3 PE8 OFF→OFF
	else					MTU23.TIOR.BIT.IOA = 0x05;	// TGRA_3 PE8 ON→OFF

	if(_pwm_duty[9]<=0)		MTU23.TIOR.BIT.IOB = 0x01;	// TGRB_3 PE9 OFF→OFF
	else					MTU23.TIOR.BIT.IOB = 0x05;	// TGRB_3 PE9 ON→OFF
	
	if(_pwm_duty[10]<=0)	MTU23.TIOR.BIT.IOC = 0x01;	// TGRC_3 PE10 OFF→OFF
	else					MTU23.TIOR.BIT.IOC = 0x05;	// TGRC_3 PE10 ON→OFF

	if(_pwm_duty[11]<=0)	MTU23.TIOR.BIT.IOD = 0x01;	// TGRD_3 PE11 OFF→OFF
	else					MTU23.TIOR.BIT.IOD = 0x05;	// TGRD_3 PE11 ON→OFF
/**/
	if(_pwm_duty[12]<=0)	MTU24.TIOR.BIT.IOA = 0x01;	// TGRA_3 PE12 OFF→OFF
	else					MTU24.TIOR.BIT.IOA = 0x05;	// TGRA_3 PE12 ON→OFF
	
	if(_pwm_duty[13]<=0)	MTU24.TIOR.BIT.IOB = 0x01;	// TGRB_4 PE13 OFF→OFF
	else					MTU24.TIOR.BIT.IOB = 0x05;	// TGRB_4 PE13 ON→OFF

	if(_pwm_duty[14]<=0)	MTU24.TIOR.BIT.IOC = 0x01;	// TGRC_4 PE14 OFF→OFF
	else					MTU24.TIOR.BIT.IOC = 0x05;	// TGRC_4 PE14 ON→OFF
	
	if(_pwm_duty[15]<=0)	MTU24.TIOR.BIT.IOD = 0x01;	// TGRD_4 PE15 OFF→OFF
	else					MTU24.TIOR.BIT.IOD = 0x05;	// TGRD_4 PE15 ON→OFF

/**/
	MTU2_syn_start();

	MTU20.TGRA = PWM_DEFAULT + _pwm_duty[0];
	MTU20.TGRB = PWM_DEFAULT + _pwm_duty[1];
//	MTU20.TGRC = PWM_DEFAULT + _pwm_duty[2];
//	MTU20.TGRD = PWM_DEFAULT + _pwm_duty[3];

//	MTU21.TGRA = PWM_DEFAULT + _pwm_duty[4];
//	MTU21.TGRB = PWM_DEFAULT + _pwm_duty[5];
//	MTU22.TGRA = PWM_DEFAULT + _pwm_duty[6];
//	MTU22.TGRB = PWM_DEFAULT + _pwm_duty[7];

	MTU23.TGRA = PWM_DEFAULT + _pwm_duty[8];
	MTU23.TGRB = PWM_DEFAULT + _pwm_duty[9];
	MTU23.TGRC = PWM_DEFAULT + _pwm_duty[10];
	MTU23.TGRD = PWM_DEFAULT + _pwm_duty[11];

	MTU24.TGRA = PWM_DEFAULT + _pwm_duty[12];
	MTU24.TGRB = PWM_DEFAULT + _pwm_duty[13];
	MTU24.TGRC = PWM_DEFAULT + _pwm_duty[14];
	MTU24.TGRD = PWM_DEFAULT + _pwm_duty[15];
}

// タイマ0〜4シンクロスタート
void MTU2_syn_start(void){
//	MTU2.TCSYSTR.BYTE = 0xF8;	// タイマ0〜4シンクロスタート
	MTU2.TCSYSTR.BYTE = 0x98;	// タイマ0,3,4シンクロスタート
}

// タイマ0〜4停止
void MTU2_syn_stop(void){
//	MTU2.TSTR.BYTE = 0x00;		// タイマ0〜4停止
	MTU2.TSTR.BYTE&=~0xC1;		// タイマ0,3,4停止
}

//1端子ずつ設定する関数
void set_pwm(short port ,short value){
	if( port>=2 && port<=7 ) return;	//間違ったポートを指定した場合は関数中断
	if( port<0 || port>15 ) return;		//間違ったポートを指定した場合は関数中断
	value=(value>1022)?1022:(value<0)?0:value;	//0-1022になるように保護
	_pwm_duty[port]=value;
}

//2端子を一度に設定する関数
void set_pwm2(short portA ,short portB ,short value){
	if( portA<0 || portA>15 ) return;	//間違ったポートを指定した場合は関数中断
	if( portB<0 || portB>15 ) return;	//間違ったポートを指定した場合は関数中断
	if( portA>=2 && portA<=7 ) return;	//間違ったポートを指定した場合は関数中断
	if( portB>=2 && portB<=7 ) return;	//間違ったポートを指定した場合は関数中断

	set_pwm(portA,value);	//プラスの時Aから出力
	set_pwm(portB,-value);	//マイナスの時Bから出力
}

//2端子を一度に設定する関数
void set_pwm2brk(short portA ,short portB ,short value){
	if( portA<0 || portA>15 ) return;	//間違ったポートを指定した場合は関数中断
	if( portB<0 || portB>15 ) return;	//間違ったポートを指定した場合は関数中断
	if( portA>=2 && portA<=7 ) return;	//間違ったポートを指定した場合は関数中断
	if( portB>=2 && portB<=7 ) return;	//間違ったポートを指定した場合は関数中断

	set_pwm(portA,1022+value);	//プラスの時Aから出力
	set_pwm(portB,1022+-value);	//マイナスの時Bから出力
}