/* System	: 二足歩行ロボット(walk)
* File		: unit_sequence.c
* function	: 単位ステップシーケンス制御
* 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		: */

#include "motor.h"
#include "unit_sequence_data.h"
#include "unit_sequence.h"

#define SMOOTHING						/* ぎこちない動きを、滑らかにします */
//#undef SMOOTHING						/* ぎこちない動きを、滑らかにします */
#define DELTA_PULSE 333						/* パルス幅変量（角度：1.4°） */

/* 公開変数 */
__f_us f_us = {1,0};						/* 単位ステップシーケンス完了フラグ */

/* 内部変数 */
static unsigned int u16_sequence_step_number = 0U;		/* 単位ステップシーケンスの実行要素番号 */
static unsigned int u16_posture_duration_counter = 0U;		/* 姿勢継続時間カウンタ(20ms単位) */
/* 目標姿勢の(パルス幅,継続時間)へのポインタ：関数の引数で必ず初期化されるため初期値を設定しない */
static unsigned int (*u16_sequence_unit)[2];
static unsigned int u16_sequence_number_max = 0U;		/* 単位ステップシーケンスの姿勢数 */

/* ポジション初期化 (BD1,2,5) */
void position_initialize(void){
	auto int i = 0;						/* 局所カウンタ変数 */
	for(i = 0;i < 4;i++){					/* パルス幅（ホームポジション）をセット */
		u16_pulse_width[i] = u16_pulse_homeposition[i];	/* 実行パルス幅を正立姿勢へ */
		u16_pulse_width_target[i] = u16_pulse_homeposition[i];/* 目標パルス幅を正立姿勢へ */
	}
}

/* 単位ステップシーケンス制御処理ユニット */
void unit_sequence_control(void){
	auto int i = 0;						/* 局所カウンタ変数 */
	/* 目標パルス幅の設定 */
	if(u16_posture_duration_counter == 0){			/* 姿勢移行時間終了 */
		if(u16_sequence_number_max == 0){
			/* 何もしない */
		}else if(u16_sequence_step_number < (u16_sequence_number_max - 1)){
			u16_sequence_step_number++;		/* 単位ステップシーケンスの実行要素番号加算 */
			/* 単位ステップシーケンス完了でなければ次の目標姿勢のパルス幅をセット */
			for(i = 0;i < 4;i++){
				u16_pulse_width_target[i] = u16_pulse_homeposition[i]
			 		+ s16_pulse_width[u16_sequence_unit[u16_sequence_step_number][0]][i];
			}
			/* 姿勢移行時間（回数×20ms）をセット */
			u16_posture_duration_counter = u16_sequence_unit[u16_sequence_step_number][1];
		}else if(u16_sequence_step_number == (u16_sequence_number_max - 1)){
			/* 単位ステップシーケンス完了 */
			u16_sequence_step_number++;		/* 次回のループで無処理とするため */
			f_us.complete = 1;			/* 単位ステップシーケンス完了フラグセット */
		}else{
			/* 処理無 */
		}
	}else{	/* 姿勢移行時間終了を待つ */
		u16_posture_duration_counter--;
	}
	/* モータ駆動制御モジュールへ実行姿勢のパルス幅を設定 */
	for(i = 0;i < 4;i++){
		/* 割込処理とu16_pulse_width[i]へのアクセスの競合が発生しないように割り込み禁止 */
		__DI();	
		/* 目標姿勢のパルス幅を実行姿勢のパルス幅に設定 */
#ifndef	SMOOTHING
		/* 目標姿勢のパルス幅を実行姿勢のパルス幅に設定 */
		u16_pulse_width[i] = u16_pulse_width_target[i];	/*  */
#else
		/* 実行姿勢のパルス幅を徐々に目標姿勢のパルス幅に近づけていく */
		if(u16_pulse_width[i] < (u16_pulse_width_target[i] - (DELTA_PULSE/2))){
			u16_pulse_width[i] += DELTA_PULSE;
		}else if(u16_pulse_width[i] > (u16_pulse_width_target[i]) + (DELTA_PULSE/2)){
			u16_pulse_width[i] -= DELTA_PULSE;
		}
#endif
		/* 範囲外値が設定されないようにする */
		if(u16_pulse_width[i] > M0UPPERLIMIT){		/* (BD1) */
			u16_pulse_width[i] = M0UPPERLIMIT;
		}
		if(u16_pulse_width[i] < M0LOWERLIMIT){		/* (BD2) */
			u16_pulse_width[i] = M0LOWERLIMIT;
		}
		__EI();
	}
}

/* 単位ステップシーケンス開始処理ユニット  (BD1,2,5) */
/* 引数：単位ステップシーケンスの配列のアドレス，配列の要素数 */
void sequence_shift_preprocess(const unsigned int u16_sunit[][2],unsigned int u16_snumber_max){
	auto int i = 0;						/* 局所カウンタ変数 */
	u16_sequence_unit = (unsigned int (*)[2])u16_sunit;	/* 単位ステップシーケンスデータ配列へのポインタを設定 */
	u16_sequence_number_max = u16_snumber_max;		/* 単位ステップシーケンスの姿勢数を設定 */
	u16_sequence_step_number = 0;				/* 単位ステップシーケンスの実行要素番号を初期化 */
	for(i = 0;i < 4;i++){
		/* 目標姿勢のパルス幅の設定 */
		u16_pulse_width_target[i] = u16_pulse_homeposition[i]
			+ s16_pulse_width[u16_sequence_unit[u16_sequence_step_number][0]][i];
	}
	/* 姿勢移行時間（回数×20ms）をセット */
	u16_posture_duration_counter = u16_sequence_unit[u16_sequence_step_number][1];
}
