#include "intMTU_rx220.h"

//初期状態ではクロックが遅い設定になっているので事前に設定を変更する事
//デューティー比を下げていくと0になる直前にデューティー比100%で出力される瞬間がある
//PWMポート数を確保するため、MTU0-2はPWMモード2を使用する
//MTU3,4はPWMモード1でのみ使用可能

// 2018/11/15 位相計数と併用した際の干渉を防ぐためMTU0〜3の要因をMTU2-TGIAからMTU0-TGIEへ変更

#define KEY_PRCR 0xA500	//プロテクト解除で使用するキー 0xA5 固定
#define PRCR_CLK 0x0001	//クロック関連
#define PRCR_MOD 0x0002	//動作モード、消費電力低減機能、ソフトウェアリセット関連
#define PRCR_LVD 0x0008	//電圧監視関連
#define PRCR_ALL (PRCR_CLK|PRCR_MOD|PRCR_LVD)	//全て

#define PROTECTOFF		(KEY_PRCR|PRCR_MOD)
#define PROTECTON		(KEY_PRCR)

#define IPL_PWM0 0x5u	//割込み優先順位 2018/11/15追加
//#define IPL_PWM2 0x5u	//割込み優先順位 2018/11/15削除
#define IPL_PWM3 0x4u	//割込み優先順位
#define IPL_PWM4 0x3u	//割込み優先順位

//PWM周期
/**//*
//20MHz 2分周期 PCLK/4 → 1カウント0.4us
//最大0xFF(65536)→26.214ms
#define PWM_INTERVAL0  2500u	//1ms
#define PWM_INTERVAL3  2500u	//1ms
//#define PWM_INTERVAL3  50000u	//20ms(SG-90用)
#define PWM_INTERVAL4  2500u	//1ms
/**/
//32MHz 1分周期 PCLK/16 → 1カウント0.5us
//最大0xFF(65536)→32.786ms
#define PWM_INTERVAL0  2000u	//1ms
//#define PWM_INTERVAL3  2000u	//1ms
#define PWM_INTERVAL3  40000u	//20ms(SG-90用)
#define PWM_INTERVAL4  2000u	//1ms
/**/

unsigned short _pwm_duty[PWM_PORT_MAX];
/*
単純PWMとして使用可能なのは
0A	0B	0C	0D
1A	1B	2A	2B
3A	3C	4A	4C	※2Aは1AまたはSCI1Tと重複するので実質使用不可能

0A:PB1
0B:PB3
0C:PA1
0D:PA3
1A:PE4
1B:PB5
2B:P27
3A:P17
3C:P16
4A:PA0
4C:PE5
*/

//PWM機能の初期化
void init_MTU(void)
{
	int i;
	//入出力の設定
	PORTA.PDR.BYTE	|= 0x0B;	//PWM機能として使用できるのはbit0,1,3
	PORTB.PDR.BYTE	|= 0x2A;	//PWM機能として使用できるのはbit1,3,5
	PORTE.PDR.BYTE	|= 0x30;	//PWM機能として使用できるのはbit1,2,4,5 ※bit1はAD09と重複
	PORT1.PDR.BYTE	|= 0xC0;	//PWM機能として使用できるのはbit4,6,7 ※4と7は共通
	PORT2.PDR.BYTE	|= 0x80;	//PWM機能として使用できるのはbit7

	//特殊機能を使用する端子の設定
	PORTA.PMR.BYTE	|= 0x0B;	//PWM機能として使用できるのはbit0,1,3
	PORTB.PMR.BYTE	|= 0x2A;	//PWM機能として使用できるのはbit1,3,5
	PORTE.PMR.BYTE	|= 0x30;	//PWM機能として使用できるのはbit1,2,4,5 ※bit1はAD09と重複
	PORT1.PMR.BYTE	|= 0xC0;	//PWM機能として使用できるのはbit4,6,7 ※4と7は共通
	PORT2.PMR.BYTE	|= 0x80;	//PWM機能として使用できるのはbit7

	//特殊機能設定
	MPC.PWPR.BIT.B0WI = 0;			//PFSレジスタの保護解除をするPFSWEレジスタの保護解除
	MPC.PWPR.BIT.PFSWE = 1;			//PFSレジスタの保護解除

	//Pxx 端子機能制御レジスタ（PxxPFS）
	MPC.P16PFS.BIT.PSEL = 0x1u;		//MTIOC_3C
	MPC.P17PFS.BIT.PSEL = 0x1u;		//MTIOC_3A
	MPC.P27PFS.BIT.PSEL = 0x1u;		//MTIOC_2B
	MPC.PA0PFS.BIT.PSEL = 0x1u;		//MTIOC_4A
	MPC.PA1PFS.BIT.PSEL = 0x1u;		//MTIOC_0B
	MPC.PA3PFS.BIT.PSEL = 0x1u;		//MTIOC_0D
	MPC.PB1PFS.BIT.PSEL = 0x1u;		//MTIOC_0C
	MPC.PB3PFS.BIT.PSEL = 0x1u;		//MTIOC_0A
	MPC.PB5PFS.BIT.PSEL = 0x2u;		//MTIOC_1B
	MPC.PE4PFS.BIT.PSEL = 0x2u;		//MTIOC_1A
	MPC.PE5PFS.BIT.PSEL = 0x1u;		//MTIOC_4C

	MPC.PWPR.BIT.PFSWE = 0;			//PFSレジスタの保護;
	MPC.PWPR.BIT.B0WI = 1;			//PFSレジスタの保護をするPFSWEレジスタの保護

	SYSTEM.PRCR.WORD = PROTECTOFF;	//プロテクト解除
	MSTP(MTU) = 0x0;	// MTUモジュール停止解除
	SYSTEM.PRCR.WORD = PROTECTON;	//プロテクト

	//割込み優先順位の設定	
	IPR(MTU0, TGIE0) = IPL_PWM0;	//2018/11/15追加
	//IPR(MTU2, TGIB2) = IPL_PWM2;	//2018/11/15削除
	IPR(MTU3, TGIB3) = IPL_PWM3;
	IPR(MTU4, TGIB4) = IPL_PWM4;
	
	//割込み許可の設定
	IEN(MTU0, TGIE0) = 0x1u;		//2018/11/15追加
	//IEN(MTU2, TGIB2) = 0x1u;		//2018/11/15削除
	IEN(MTU3, TGIB3) = 0x1u;
	IEN(MTU4, TGIB4) = 0x1u;
	
	//割込みフラグのクリア
	IR(MTU0, TGIE0) = 0x0;			//2018/11/15追加
	//IR(MTU2, TGIB2) = 0x0;		//2018/11/15削除
	IR(MTU3, TGIB3) = 0x0;	
	IR(MTU4, TGIB4) = 0x0;	

	MTU.TRWER.BIT.RWE = 0x1u;	//MTU書込み許可
	MTU.TSYR.BYTE	= 0x07;		//タイマ0−2同期

	//クロックの設定 (SYSTEM.SCKCR.BIT.PCKBに依存?)
/**//*
	//20MHz,2分周期
	//0:PCLK/1  0.1us	1ms＝10000カウント
	//1:PCLK/4  0.4us	1ms＝2500カウント
	//2:PCLK/16 1.6us	1ms＝625カウント
	MTU0.TCR.BIT.TPSC = 0x1;
	MTU1.TCR.BIT.TPSC = 0x1;
	MTU2.TCR.BIT.TPSC = 0x1;

	MTU3.TCR.BIT.TPSC = 0x1;
	MTU4.TCR.BIT.TPSC = 0x1;
/**/
	//32MHz,1分周期
	//0:PCLK/1  0.03125us	1ms＝32000カウント
	//1:PCLK/4  0.125us	1ms＝8000カウント
	//2:PCLK/16 0.5us	1ms＝2000カウント
	MTU0.TCR.BIT.TPSC = 0x2;
	MTU1.TCR.BIT.TPSC = 0x2;
	MTU2.TCR.BIT.TPSC = 0x2;

	MTU3.TCR.BIT.TPSC = 0x2;
	MTU4.TCR.BIT.TPSC = 0x2;
/**/	
	//クロックカウントの設定
	MTU0.TCR.BIT.CKEG = 0x1u;	//立下りでカウント
	MTU1.TCR.BIT.CKEG = 0x1u;
	MTU2.TCR.BIT.CKEG = 0x1u;

	MTU3.TCR.BIT.CKEG = 0x1u;
	MTU4.TCR.BIT.CKEG = 0x1u;
	
	//カウンタクリア要因の設定
	MTU0.TCR.BIT.CCLR = 0x0u; //自動クリアなし(TGIE0割込み時に自分でクリア)
	MTU1.TCR.BIT.CCLR = 0x0u; //自動クリアなし
	MTU2.TCR.BIT.CCLR = 0x0u; //自動クリアなし
	//MTU0.TCR.BIT.CCLR = 0x3u; //同期クリア
	//MTU1.TCR.BIT.CCLR = 0x3u; //同期クリア
	//MTU2.TCR.BIT.CCLR = 0x1u; //TGRAとのコンペアマッチ

	MTU3.TCR.BIT.CCLR = 0x2u; //TGRBとのコンペアマッチ
	MTU4.TCR.BIT.CCLR = 0x2u; //TGRBとのコンペアマッチ

	//PWMモードの設定
	MTU0.TMDR.BYTE = 0x03u;	//0x02:PWM1 0x03:PWM2
	MTU1.TMDR.BYTE = 0x03u;	
	MTU2.TMDR.BYTE = 0x03u;	

	MTU3.TMDR.BYTE = 0x02u;	
	MTU4.TMDR.BYTE = 0x02u;	
	
	
	//端子の出力方法設定
	// 1 Low出力→Low出力
	// 2 Low出力→High出力
	// 3 Low出力→トグル出力
	// 4 出力禁止
	// 5 High出力→Low出力
	// 6 High出力→High出力
	// 7 High出力→トグル出力
/**/
	MTU0.TIORH.BIT.IOA = 0x1u;	//L -> L
	MTU0.TIORH.BIT.IOB = 0x1u;	//L -> L
	MTU0.TIORL.BIT.IOC = 0x1u;	//L -> L
	MTU0.TIORL.BIT.IOD = 0x1u;	//L -> L

	MTU1.TIOR.BIT.IOA = 0x1u;	//L -> L
	MTU1.TIOR.BIT.IOB = 0x1u;	//L -> L

	MTU2.TIOR.BIT.IOA = 0x1u;	//L -> L
	MTU2.TIOR.BIT.IOB = 0x1u;	//L -> L
	

	MTU3.TIORH.BIT.IOA = 0x1u;	//L -> L
	MTU3.TIORH.BIT.IOB = 0x1u;	//L -> L
	MTU3.TIORL.BIT.IOC = 0x1u;	//L -> L
	MTU3.TIORL.BIT.IOD = 0x1u;	//L -> L

	MTU4.TIORH.BIT.IOA = 0x1u;	//L -> L
	MTU4.TIORH.BIT.IOB = 0x1u;	//L -> L
	MTU4.TIORL.BIT.IOC = 0x1u;	//L -> L
	MTU4.TIORL.BIT.IOD = 0x1u;	//L -> L
/**//*
	MTU0.TIORH.BIT.IOA = 0x5u;	//H -> L
	MTU0.TIORH.BIT.IOB = 0x5u;	//H -> L
	MTU0.TIORL.BIT.IOC = 0x5u;	//H -> L
	MTU0.TIORL.BIT.IOD = 0x5u;	//H -> L

	MTU1.TIOR.BIT.IOA = 0x5u;	//H -> L
	MTU1.TIOR.BIT.IOB = 0x5u;	//H -> L

	MTU2.TIOR.BIT.IOA = 0x5u;	//H -> L
	MTU2.TIOR.BIT.IOB = 0x5u;	//H -> L
	

	MTU3.TIORH.BIT.IOA = 0x5u;	//H -> L
	MTU3.TIORH.BIT.IOB = 0x2u;	//L -> H
	MTU3.TIORL.BIT.IOC = 0x5u;	//H -> L
	MTU3.TIORL.BIT.IOD = 0x2u;	//L -> H

	MTU4.TIORH.BIT.IOA = 0x5u;	//L -> L
	MTU4.TIORH.BIT.IOB = 0x2u;	//L -> L
	MTU4.TIORL.BIT.IOC = 0x5u;	//L -> L
	MTU4.TIORL.BIT.IOD = 0x2u;	//L -> L
/**/
	MTU0.TGRA = 0;
	MTU0.TGRB = 0;
	MTU0.TGRC = 0;
	MTU0.TGRD = 0;
	MTU0.TGRE = PWM_INTERVAL0;		//2018/11/15追加 PWM周期

	MTU1.TGRA = 0;
	MTU1.TGRB = 0;

	//MTU2.TGRA = PWM_INTERVAL0;	//2018/11/15削除
	MTU2.TGRB = 0;

	MTU3.TGRA = 0;
	MTU3.TGRB = PWM_INTERVAL3;	//PWM周期
	MTU3.TGRC = 0;
	MTU3.TGRD = PWM_INTERVAL3;

	MTU4.TGRA = 0;
	MTU4.TGRB = PWM_INTERVAL4;	//PWM周期
	MTU4.TGRC = 0;
	MTU4.TGRD = PWM_INTERVAL4;

	MTU.TOER.BIT.OE4A = 0x1u;	//TIOC_4A出力許可
	MTU.TOER.BIT.OE4C = 0x1u;	//TIOC_4C出力許可

	//カウント開始	
	MTU.TSTR.BIT.CST0 = 0x1u;
	MTU.TSTR.BIT.CST1 = 0x1u;
	MTU.TSTR.BIT.CST2 = 0x1u;
	MTU.TSTR.BIT.CST3 = 0x1u;
	MTU.TSTR.BIT.CST4 = 0x1u;
	
	//割込み許可
	MTU0.TIER2.BIT.TGIEE = 0x1u;	//2018/11/15追加
	//MTU2.TIER.BIT.TGIEA = 0x1u;	//2018/11/15削除
	MTU3.TIER.BIT.TGIEB = 0x1u;
	MTU4.TIER.BIT.TGIEB = 0x1u;
	
	for(i=0;i<PWM_PORT_MAX;i++){
		_pwm_duty[i]=0;
	}
}

//MTU2 TGRAコンペアマッチ割込み intprg.c Excep_MTU22_TGIA2内で実行させる
//void MTU22_INT_TGIA2(void){
//MTU0 TGREコンペアマッチ割込み intprg.c Excep_MTU20_TGIE0内で実行させる
void MTU20_INT_TGIE0(void){		//2018/11/15変更

	//カウント停止
	MTU.TSTR.BIT.CST0 = 0x0u;
	MTU0.TCNT=0;

	//PWM設定の場合のみカウンタクリア
	if(MTU1.TMDR.BYTE==0x03){
		MTU.TSTR.BIT.CST1 = 0x0u;
		MTU1.TCNT=0;
	}
	if(MTU2.TMDR.BYTE==0x03){
		MTU.TSTR.BIT.CST2 = 0x0u;
		MTU2.TCNT=0;
	}
	//フラグクリア
	IR(MTU0, TGIE0) = 0x0;

	//出力が小さいと挙動が不安定(割込み関連)
	if(_pwm_duty[PWM_PORT_0A]<=12)	MTU0.TIORH.BIT.IOA = 0x1u;	//L -> L
	else							MTU0.TIORH.BIT.IOA = 0x5u;	//H -> L
	if(_pwm_duty[PWM_PORT_0B]<=12)	MTU0.TIORH.BIT.IOB = 0x1u;	//L -> L
	else							MTU0.TIORH.BIT.IOB = 0x5u;	//H -> L
	if(_pwm_duty[PWM_PORT_0C]<=12)	MTU0.TIORL.BIT.IOC = 0x1u;	//L -> L
	else							MTU0.TIORL.BIT.IOC = 0x5u;	//H -> L
	if(_pwm_duty[PWM_PORT_0D]<=12)	MTU0.TIORL.BIT.IOD = 0x1u;	//L -> L
	else							MTU0.TIORL.BIT.IOD = 0x5u;	//H -> L

	if(_pwm_duty[PWM_PORT_1A]<=12)	MTU1.TIOR.BIT.IOA = 0x1u;	//L -> L
	else							MTU1.TIOR.BIT.IOA = 0x5u;	//H -> L
	if(_pwm_duty[PWM_PORT_1B]<=12)	MTU1.TIOR.BIT.IOB = 0x1u;	//L -> L
	else							MTU1.TIOR.BIT.IOB = 0x5u;	//H -> L

	if(_pwm_duty[PWM_PORT_2B]<=12)	MTU2.TIOR.BIT.IOB = 0x1u;	//L -> L
	else							MTU2.TIOR.BIT.IOB = 0x5u;	//H -> L

	//デューティー比を設定
	MTU0.TGRA=_pwm_duty[PWM_PORT_0A];
	MTU0.TGRB=_pwm_duty[PWM_PORT_0B];
	MTU0.TGRC=_pwm_duty[PWM_PORT_0C];
	MTU0.TGRD=_pwm_duty[PWM_PORT_0D];
	MTU1.TGRA=_pwm_duty[PWM_PORT_1A];
	MTU1.TGRB=_pwm_duty[PWM_PORT_1B];
	MTU2.TGRB=_pwm_duty[PWM_PORT_2B];

	//カウント開始
	MTU.TSTR.BIT.CST0 = 0x1u;

	//PWM設定の場合のみカウンタ開始
	if(MTU1.TMDR.BYTE==0x03){
		MTU.TSTR.BIT.CST1 = 0x1u;
	}
	if(MTU2.TMDR.BYTE==0x03){
		MTU.TSTR.BIT.CST2 = 0x1u;
	}
}

//MTU3 TGRBコンペアマッチ割込み intprg.c Excep_MTU23_TGIB3内で実行させる
void MTU23_INT_TGIB3(void){
	
	//カウント停止
	MTU.TSTR.BIT.CST3 = 0x0u;

	//フラグクリア
	IR(MTU3, TGIB3) = 0x0;

	//出力が小さいと挙動が不安定(割込み関連)
	if(_pwm_duty[PWM_PORT_3A]<=25){
		MTU3.TIORH.BIT.IOA = 0x1u;	//L -> L
		MTU3.TIORH.BIT.IOB = 0x1u;	//L -> L
	}else{
		MTU3.TIORH.BIT.IOA = 0x5u;	//H -> L
		MTU3.TIORH.BIT.IOB = 0x2u;	//L -> H
	}
	if(_pwm_duty[PWM_PORT_3C]<=25){
		MTU3.TIORL.BIT.IOC = 0x1u;	//L -> L
		MTU3.TIORL.BIT.IOD = 0x1u;	//L -> L
	}else{
		MTU3.TIORL.BIT.IOC = 0x5u;	//H -> L
		MTU3.TIORL.BIT.IOD = 0x2u;	//L -> H
	}
	//デューティー比を設定
	MTU3.TGRA=_pwm_duty[PWM_PORT_3A];
	MTU3.TGRC=_pwm_duty[PWM_PORT_3C];

	//カウント開始
	MTU.TSTR.BIT.CST3 = 0x1u;

}

//MTU4 TGRBコンペアマッチ割込み intprg.c Excep_MTU24_TGIB4内で実行させる
void MTU24_INT_TGIB4(void){

	//カウント停止
	MTU.TSTR.BIT.CST4 = 0x0u;

	//フラグクリア
	IR(MTU4, TGIB4) = 0x0;

	//出力が小さいと挙動が不安定(割込み関連)
	if(_pwm_duty[PWM_PORT_4A]<=25){
		MTU4.TIORH.BIT.IOA = 0x1u;	//L -> L
		MTU4.TIORH.BIT.IOB = 0x1u;	//L -> L
	}else{
		MTU4.TIORH.BIT.IOA = 0x5u;	//H -> L
		MTU4.TIORH.BIT.IOB = 0x2u;	//L -> H
	}
	if(_pwm_duty[PWM_PORT_4C]<=25){
		MTU4.TIORL.BIT.IOC = 0x1u;	//L -> L
		MTU4.TIORL.BIT.IOD = 0x1u;	//L -> L
	}else{
		MTU4.TIORL.BIT.IOC = 0x5u;	//H -> L
		MTU4.TIORL.BIT.IOD = 0x2u;	//L -> H
	}

	//デューティー比を設定
	MTU4.TGRA=_pwm_duty[PWM_PORT_4A];
	MTU4.TGRC=_pwm_duty[PWM_PORT_4C];

	//カウント開始
	MTU.TSTR.BIT.CST4 = 0x1u;

}

void set_pwm(short port ,short value){
	unsigned short pwm_interval=0;
	short k=0;
	unsigned short u_value;
	if( port<0 || PWM_PORT_MAX<=port ) return;	//間違ったポートを指定した場合は関数中断

	value=(value>1000)?1000:(value<=0)?0:value;		
	u_value=value;

	//入力値保護
	if(		  PWM_PORT_0A<=port && port<=PWM_PORT_2B){
		pwm_interval=PWM_INTERVAL0;
	}else if( PWM_PORT_3A<=port && port<=PWM_PORT_3C){
		pwm_interval=PWM_INTERVAL3;
	}else if( PWM_PORT_4A<=port && port<=PWM_PORT_4C){
		pwm_interval=PWM_INTERVAL4;
	}
	k=(short)(((long)pwm_interval)*10/1000);
	u_value=(u_value*k)/10;
	_pwm_duty[port]=u_value;
}

void set_pwm2(short port_A ,short port_B ,short value){
	if( port_A<0 || PWM_PORT_MAX<=port_A ) return;	//間違ったポートを指定した場合は関数中断
	if( port_B<0 || PWM_PORT_MAX<=port_B ) return;	//間違ったポートを指定した場合は関数中断
	
	value=(value>1000)?1000:(value<-1000)?-1000:value;

	if(value>0){
		set_pwm(port_A,value);
		set_pwm(port_B,0);
	}else if(value<0){
		set_pwm(port_A,0);
		set_pwm(port_B,-value);
	}else{
		set_pwm(port_A,0);
		set_pwm(port_B,0);
	}
}

//中立時回生ブレーキ仕様
void set_pwm3(short port_A ,short port_B ,short value){
	if( port_A<0 || PWM_PORT_MAX<=port_A ) return;	//間違ったポートを指定した場合は関数中断
	if( port_B<0 || PWM_PORT_MAX<=port_B ) return;	//間違ったポートを指定した場合は関数中断
	
	value=(value>1000)?1000:(value<-1000)?-1000:value;

	if(value>0){
		set_pwm(port_A,value);
		set_pwm(port_B,0);
	}else if(value<0){
		set_pwm(port_A,0);
		set_pwm(port_B,-value);
	}else{
		set_pwm(port_A,1000);
		set_pwm(port_B,1000);
	}
}

//常時回生ブレーキ仕様	※2019/02/25追加
void set_pwm2brk(short port_A ,short port_B ,short value){
	if( port_A<0 || PWM_PORT_MAX<=port_A ) return;	//間違ったポートを指定した場合は関数中断
	if( port_B<0 || PWM_PORT_MAX<=port_B ) return;	//間違ったポートを指定した場合は関数中断
	
	value=(value>1000)?1000:(value<-1000)?-1000:value;

	if(value>0){
		set_pwm(port_A,1000);
		set_pwm(port_B,1000-value);
	}else if(value<0){
		set_pwm(port_A,1000+value);
		set_pwm(port_B,1000);
	}else{
		set_pwm(port_A,1000);
		set_pwm(port_B,1000);
	}
}

#define SERVO_TIME_SAMPLE	20000L
#define SERVO_TIME_MIN	500L
#define SERVO_TIME_MAX	2400L
#define SERVO_TIME_MID	((SERVO_TIME_MIN+SERVO_TIME_MAX)/2)
#define SERVO_DUTY_MIN	25L
#define SERVO_DUTY_MAX	120L
#define SERVO_DUTY_MID	73L
#define SERVO_DUTY_SPAN	(SERVO_DUTY_MAX-SERVO_DUTY_MIN)
void set_servo(short port_A,short angle){
	long value;
	if( port_A<0 || PWM_PORT_MAX<=port_A ) return;	//間違ったポートを指定した場合は関数中断
	angle=(angle>900)?900:(angle<-900)?-900:angle;

	value=(long)angle*SERVO_DUTY_SPAN/1800L;
	value+=SERVO_DUTY_MID;
	value=(value>SERVO_DUTY_MAX)? SERVO_DUTY_MAX:
		  (value<SERVO_DUTY_MIN)? SERVO_DUTY_MIN:value;
	set_pwm(port_A,value);
}