#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

#define STOP_TIME 1000

DeclareTask(Task1);				/* Task1を宣言 */

void ecrobot_device_initialize(){		/* OSEK起動時の処理（モータ停止）*/
	nxt_motor_set_speed(NXT_PORT_B,0,1);
	nxt_motor_set_speed(NXT_PORT_C,0,1);
}

void ecrobot_device_terminate(){		/* OSEK終了時の処理（モータ停止）*/
	nxt_motor_set_speed(NXT_PORT_B,0,1);
	nxt_motor_set_speed(NXT_PORT_C,0,1);
}

void user_1ms_isr_type2(void){}

/* 直進動作の関数定義 str_enc()
左右のモータの回転角の平均値が ref_angle になるまで、
左右のモータがそれぞれ duty_l, duty_r のduty比で回転する
*/
void str_enc(int ref_angle, int duty_l, int duty_r){

	int angle_b, angle_c, angle;

	// ロータリエンコーダリセット 
    	nxt_motor_set_count(NXT_PORT_B,0);
	nxt_motor_set_count(NXT_PORT_C,0);

	nxt_motor_set_speed(NXT_PORT_B,duty_r,1);
	nxt_motor_set_speed(NXT_PORT_C,duty_l,1);

    	while(1){

		angle_b = nxt_motor_get_count(NXT_PORT_B);
		angle_c = nxt_motor_get_count(NXT_PORT_C);	
		angle = (angle_b + angle_c ) /2;

		//モータの平均回転角度が指定角度（前進時：以上、後退時：以下）になったらループを出る
		if( ref_angle >= 0){
			if( angle >= ref_angle ) break; //前進時
		} else {
			if( angle <= ref_angle ) break; //後退時
		}

		systick_wait_ms(10);
	}

}

TASK(Task1)
{

	float ref_angle;
	int duty_l, duty_r;

	//前進動作
	ref_angle = 720;
	//duty比は前項の「モータの使用」で調整した値を用いる
	duty_l = 60; duty_r = 60;
	str_enc((int)ref_angle, duty_l, duty_r);

	//モータ停止
	nxt_motor_set_speed(NXT_PORT_B,0,1);
	nxt_motor_set_speed(NXT_PORT_C,0,1);
	systick_wait_ms(STOP_TIME); 

	//後退動作
	ref_angle = -720;
	duty_l = -60, duty_r = -60;
	str_enc((int)ref_angle, duty_l, duty_r);

	//モータ停止
	nxt_motor_set_speed(NXT_PORT_B,0,1);
	nxt_motor_set_speed(NXT_PORT_C,0,1);
	systick_wait_ms(STOP_TIME); 

	display_string("THAT'S ALL !");	
	display_update();
	TerminateTask();			/* 処理終了 */

}