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

DeclareCounter(SysTimerCnt);
DeclareResource(resource1);
DeclareTask(Task1);				/* Task1を宣言 */
DeclareTask(Task_bg);				/* Task_bgを宣言 */

int i=0;

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

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

void user_1ms_isr_type2(void){
    SignalCounter(SysTimerCnt);		/* カウンタ簡易版 */
}
#define SPEED 60
#define TURN_CNT 40
#define MOVE_CNT 120

#define TURN_TIME 500
#define MOVE_TIME 1000

TASK(Task1)
{
	if(i <= MOVE_CNT){
		nxt_motor_set_speed(L_MOTOR,SPEED,1);
		nxt_motor_set_speed(R_MOTOR,SPEED,1);
	}else if(i > MOVE_CNT && i <= TURN_CNT+MOVE_CNT){
		nxt_motor_set_speed(L_MOTOR,SPEED,1);
		nxt_motor_set_speed(R_MOTOR,-1*SPEED,1);	
	}else{
		i=0;
	}
	
	display_clear(0);
	display_goto_xy(0,1);
   	display_string("I=");		/* 変数の内容を表示 */
	display_int(i, 5);
	display_update();
	
	i++;
	
	TerminateTask();					/* 処理終了 */
}

TASK(Task_bg){
	while(1){
		if(ecrobot_get_touch_sensor(PORT_TOUCH) == 1){
			GetResource(resource1);
			nxt_motor_set_speed(L_MOTOR,-1*SPEED,1);
			nxt_motor_set_speed(R_MOTOR,-1*SPEED,1);
			systick_wait_ms(MOVE_TIME);
			nxt_motor_set_speed(L_MOTOR,SPEED,1);
			nxt_motor_set_speed(R_MOTOR,-1*SPEED,1);
			systick_wait_ms(TURN_TIME);
			nxt_motor_set_speed(L_MOTOR,0,1);
			nxt_motor_set_speed(R_MOTOR,0,1);
			ReleaseResource(resource1);
		}
	}
	
	TerminateTask();
}