/* datalogging.c */ 
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

/* OSEK declarations */
DeclareCounter(SysTimerCnt);
DeclareTask(Task1);

/* LEJOS OSEK hooks */
void ecrobot_device_initialize()
{
	ecrobot_init_sonar_sensor(NXT_PORT_S2);
	
	// デバイス名を設定します///////////////////////////
	// デバイス名は重複しないように ET + チームIDとします
	if(ecrobot_get_bt_status()==BT_NO_INIT){
		ecrobot_set_bt_device_name("ET500");
	}
	////////////////////////////////////////////////////
	
	// bluetooth通信のスレーブデバイスとして初期化します
	// 引数はパスキーです
	ecrobot_init_bt_slave("LEJOS-OSEK");
}

void ecrobot_device_terminate()
{
	ecrobot_term_sonar_sensor(NXT_PORT_S2);
	
	// bluetooth通信終了処理を行います
	ecrobot_term_bt_connection();
}

/* LEJOS OSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void)
{
	StatusType ercd;

	ercd = SignalCounter(SysTimerCnt); /* Increment OSEK Alarm Counter */
	if (ercd != E_OK)
	{
		ShutdownOS(ercd);
	}
}

/* Task1 executed every 50msec */
TASK(Task1)
{
	static S8 i,j;
	int sonar;

	sonar = ecrobot_get_sonar_sensor(NXT_PORT_S2);

	// NXTの全ポートに接続されたセンサおよびモータ(回転角度)の
	// データおよび内部状態データ(システムタイマー、バッテリ電圧) 等を
	// 送信します。
	ecrobot_bt_data_logger(i++, j--);

	/* display Sensors/Motors/NXT internal status */ 
	ecrobot_status_monitor("Data Logging");

	TerminateTask();
}
