/* モータドライバ用ヘッダ */
#include "dev_mtr.h"

/*
 * モータの回転制御用ポートの初期化
 * マルチファンクション機能を初期化
 * 初期値は両方のモーターがSTOP状態
 */
void mtr_initialize(void)
{	
	/*
	 * ポートの初期化
	 */
	 
	*PD_CR0 = *PD_CR0 & 0xFF00;
	*PD_CR1 = *PD_CR1 & 0xFF00;
	
	/* PD0,1,4,5を出力設定 */
	*PD_IOR0 = *PD_IOR0 | 0x0033;
	
	/* 停止状態に設定 */
	*PD_DR0 = *PD_DR0 & 0xFFCC;
	
	/* PWM端子機能をTIOC4A,TIOC4Cに設定 */
	*PC_CR1 = (*PC_CR1 & 0xCFCF) | 0x2020;
	
	/* TIOC4A,TIOC4Cを出力に設定 */
	*PC_IOR0 = 0x00A0;						
	
	/*
	 *PWMの設定
	 */
	/*カウンタクリア要因はTGRD、立ち下がりでカウント、カウント=内部クロック/4 */	
	*TCR_4 = 0xC1;
	
	/* PWMモード1に設定 */
	*TMDR_4 = 0x02;
	
	/* PWM端子出力を許可する */					
	*TOER = *TOER | 0x36;
	
	/* 初期出力は1、TGRAコンペアマッチで0出力、TGRBコンペアマッチで1出力 */
	*TIORH_4 = 0x65;
	
	/* 初期出力は1、TGRCコンペアマッチで0出力、TGRDコンペアマッチで1出力 */
	*TIORL_4 = 0x65;	
	
	/* デューティ比の決定
	 * 周期1kHz */
	*TGRA_4 = 0;
	*TGRB_4 = 6000;
	*TGRC_4 = 0;
	*TGRD_4 = 6000;
	
	/* PWMのカウンタスタート */
	*TSTR = *TSTR | 0x80;
}

/*
 * 左右モータの回転制御と出力の調整
 * 回転制御の引数が異常な場合はモータを停止
 * 引数  ：左モータの回転，左モータの出力
   　　　：右モータの回転，右モータの出力
 * 戻り値：なし
  */
void mtr_set(int mtr_l_aspect, int mtr_l_power, int mtr_r_aspect, int mtr_r_power)
{
	
	switch(mtr_l_aspect){
	case GO:
		*PD_DR0 = (*PD_DR0 & 0xFFFC) | 0x0001;
		*TGRA_4 = (int)(3000 * ((double)mtr_l_power / 100));
		break;
	case BACK:
		*PD_DR0 = (*PD_DR0 & 0xFFFC) | 0x0002;
		*TGRA_4 = (int)(3000 * ((double)mtr_l_power / 100));
		break;
	case BRAKE:
		*PD_DR0 = *PD_DR0 | 0x0003;
		break;
	default:
		*PD_DR0 = *PD_DR0 & 0xFFFC;
		break;
	}

	switch(mtr_r_aspect){
	case GO:
		*PD_DR0 = (*PD_DR0 & 0xFFCF) | 0x0010;
		*TGRC_4 = (int)(3000 * ((double)mtr_r_power / 100));
		break;
	case BACK:
		*PD_DR0 = (*PD_DR0 & 0xFFCF) | 0x0020;
		*TGRC_4 = (int)(3000 * ((double)mtr_r_power / 100));
		break;
	case BRAKE:
		*PD_DR0 = *PD_DR0 | 0x0030;
		break;
	default:
		*PD_DR0 = *PD_DR0 & 0xFFCF;
		break;
	}
}
