/* System	: 二足歩行ロボット(walk)
* File		: motor.c
* function	: モータ駆動制御、メイン周期(20ms)生成
* Version	: 1.0
* Date		: 2019/5/7
* CPU		: RL78/G12 ROM:16KB RAM:1.5KB fCLK:24MHz
* IDE		: CS+ for CC V6.01.00 [01 Dec 2017]
* Programmer	: Hajime Sugiyama
* Note		: */

/* 特殊機能レジスタ（SFR）へのアクセス記述を使用する(RL78/R5F1026A) */
#include "iodefine.h"
/* チャネル0と1のタイマ割込み関数を設定します */
#pragma interrupt tau0_channel0_interrupt(vect = INTTM00)
#pragma interrupt tau0_channel1_interrupt(vect = INTTM01)

#include "motor.h"

/* 公開変数 */
struct _F_TIMER f_timer = {0,0};		/* 定周期到達情報（20ms経過フラグ） */
/* 以下の配列は "unit_sequence.c" の position_initialize() で初期化される */
unsigned int u16_pulse_width[4];		/* 実行姿勢のパルス幅 */

/* モータ駆動制御初期化処理ユニット (SE4,BD6) */
void motor_initialize(void){
	/* タイマユニット0の設定初期化 */
	TAU0EN = 1U;				/* タイマ・アレイ・ユニットのクロック供給 TAU0EN(PER0.bit0) */
	TPS0 = 0x0010U;				/* タイマ・クロック選択 CK01:12MHz,CK00:24MHz(fCLK24MHz) */
	TT0 = 0x0A0FU;				/* すべてのタイマのカウント動作停止タイマ･チャネル停止 */

	/* すべてのタイマの割込禁止・要求フラグクリア */
	TMMK00 = 1U;				/* INTTM00 割込みの禁止 */
	TMIF00 = 0U;				/* INTTM00 割込み要求フラグのクリア */
	TMMK01 = 1U;				/* INTTM01 割込みの禁止 */
	TMIF01 = 0U;				/* INTTM01 割込み要求フラグのクリア */
	TMMK01H = 1U;				/* INTTM01H 割込みの禁止 */
	TMIF01H = 0U;				/* INTTM01H 割込み要求フラグのクリア */
	TMMK02 = 1U;				/* INTTM02 割込みの禁止 */
	TMIF02 = 0U;				/* INTTM01H 割込み要求フラグのクリア */
	TMMK03 = 1U;				/* INTTM03 割込みの禁止 */
	TMIF03 = 0U;				/* INTTM01H 割込み要求フラグのクリア */
	TMMK03H = 1U;				/* INTTM03H 割込みの禁止 */
	TMIF03H = 0U;				/* INTTM01H 割込み要求フラグのクリア */

	TMPR100 = 1U;				/* INTTM00 優先順位レベル3（低優先順位） */
	TMPR000 = 1U;

	TMPR101 = 1U;				/* INTTM01 優先順位レベル3（低優先順位） */
	TMPR001 = 1U;
	
	TMR00 = 0x8000U;			/* タイマチャネル0:インターバル・タイマ・モード,動作クロックCK01(12MHz) */
	TDR00 = 0xEA5FU;			/* インターバル 5ms(59999/12MHz) */
	TO0 &= ~0x0001U;			/* TO00をポート機能として使用 */
	TOE0 &= ~0x0001U;			/* TO00のタイマの出力を禁止 */

	TMR01 = 0x0000U;			/* タイマチャネル1:インターバル・タイマ・モード,動作クロックCK00(24MHz) (BD1,2) */
	TDR01 = 0x79DFU;			/* 1300μs(31199/24MHz) */
	TOM0 &= ~0x0002U;			/* タイマチャネル1を単独チャネル動作機能として使用 */
	TOL0 &= 0x0002U;			/* レジスタ設定は無効であるが、念のため正論理出力(アクティブ・ハイ)に設定 */
	TO0 &= ~0x0002U;			/* TO01をポート機能として使用 */
	TOE0 &= ~0x0002U;			/* TO01のタイマの出力を禁止 */

	/* ＲＣサーボモータＰＯＲＴ設定初期化 */
	P1 &= 0xF0;				/* ﾓｰﾀ出力停止 */
	PMC1 &= 0xF0;				/* ポート10〜13をデジタル入出力に設定 */
	PM1 &= 0xF0;				/* ポート10〜13を出力に設定 */
}

/* 定周期生成タイマ起動処理ユニット（5ms） (SE4) */
void interval_timer5ms_start(void){
	TMIF00 = 0U;				/* INTTM00 割込み要求フラグのクリア */
	TMMK00 = 0U;				/* INTTM00 割込みの許可 */
	/* タイマ・チャネル開始レジスタ0（TS0） */
	TS0 |= 0x0001U;				/* カウント動作許可(TE00ビットを1にセット) */
}


/* 定周期生成タイマ割込処理ユニット（5ms） */
static void tau0_channel0_interrupt(void){
	static unsigned char u8_motor_no = 0U;	/* モータ番号（0〜3） */
	static unsigned char u8_mortor_out = 0U;/* モータ出力バッファ */
	/* メインループ周期(20ms)の生成 */
	if(u8_motor_no == 0){
		f_timer.main = 1U;		/* 20ms経過 */
	}
	/* RCサーボモータ駆動周期(5ms)の処理 (SE4,BD1,2,5,6) */
	TDR01 = u16_pulse_width[u8_motor_no];	/* パルス幅設定 */
	u8_mortor_out = 0x01 << u8_motor_no;	/* ポート選定 */
	/*【注】コンパイル時warning表示⇒u8_motornoは4未満のため問題なし */
	P1 |= u8_mortor_out;			/* ﾓｰﾀ出力開始 */
	/* パルス幅生成タイマ開始 */
	TMIF01 = 0U;				/* INTTM01 割込み要求フラグのクリア */
	TMMK01 = 0U;				/* INTTM01 割込みの許可 */
	TS0 |= 0x0002U;				/* カウント動作許可(TE01ビットを1にセット) */
	if(++u8_motor_no >= 4){			/* モータ番号変更（0〜3） */
		u8_motor_no = 0U;
	}
}

/* パルス幅生成タイマ割込み処理ユニット (BD6) */
static void tau0_channel1_interrupt(void){
	P1 &= 0xF0;				/* モータパルス出力停止 */
	/* パルス幅生成タイマ停止 */
						/* タイマ・チャネル停止レジスタ0（TT0） */
	TT0 |= 0x0002U;				/* カウント動作停止(TE01ビットを0にクリア) */
	TMMK01 = 1U;				/* INTTM01 割込みの禁止 */
	TMIF01 = 0U;				/* INTTM00 割込み要求フラグのクリア */
}
