void
_kernel_call_inirtn(void)
{
	KzAttIni( (VP_INT)(0) );
	timer_initialize( (VP_INT)(0) );
	serial_initialize( (VP_INT)(0) );
}

int main(void)
{
    vuint32_t PulseNum = 0;
    
    /* 模块初始化 */
    exc_init();                                          /* 中断初始化 */
    light_init();                                        /* LED灯初始化 */
    
    /* 初始化串口 */
    serial_initialize((intptr_t)(NULL));
    
    ftm_decoder_init(PTA8_9);

    printf("%s\n","FTM - PWM test!");
   
    while(1)
    {
        PulseNum = FTM_CNT_REG(FTM1_BASE_PTR); //读FTM1的CNT寄存器的值
        
        if(PulseNum > 5000)
        {
            light_change(LIGHT1);
        }
    }
    return 0;
}

int main(void)
{
    /* 模块初始化 */
    exc_init();                                          /* 中断初始化 */
    light_init();                                        /* LED灯初始化 */

    /* 初始化串口 */
    serial_initialize((intptr_t)(NULL));
    
    pit_init(PIT0, BUS_CLK_50000*1000); 
    pit_enable(PIT0);  
    
    printf("%s\n","PIT test!");

    /* 主循环 */
    while (1)
    {
        //主循环计数到一定的值，使小灯的亮、暗状态切换
        if (pit_count > 0)
        {
            light_change(LIGHT0);//指示灯的亮、暗状态切换
            pit_count = 0;
        }
        pit_enable(PIT0);
    } //end_while
}

int main(int argc, char **argv)
{   
    /* 模块初始化 */
    exc_init();                                          /* 中断初始化 */
    sys_timer_init();                                    /* 系统时钟初始化 */
    light_init();                                        /* LED灯初始化 */
    switch_init();                                       /* 开关初始化 */
    speaker_init();                                      /* 蜂鸣器初始化 */
    motor_init();                                        /* 电机初始化 */
    steer_init();                                        /* 舵机初始化 */
    decoder_init();                                      /* 编码器初始化 */
    serial_initialize((intptr_t)(NULL));                 /* 初始化串口 */
    //sd_init(&Fatfs);                                   /* 初始化SD卡，并创建文件 */
    //sd_create_file(&test_data, test_data_name);
    
    /* 命令注册 */
    help_cmd_initialize((intptr_t)(NULL));
    light_cmd_initialize((intptr_t)(NULL));
    switch_cmd_initialize((intptr_t)(NULL));
    speaker_cmd_initialize((intptr_t)(NULL));
    motor_cmd_initialize((intptr_t)(NULL));
    decoder_cmd_initialize((intptr_t)(NULL));
    control_cmd_initialize((intptr_t)(NULL));
    //sd_cmd_initialize((intptr_t)(NULL));

    printf("\n Welcome to k60 software platform!");
    printf("\n Press 'help' to get the help! \n");
    
    //light_open(LIGHT4); 

    /* ntshell测试 */
    task_ntshell((intptr_t)(NULL));
}

void firmware_init(){
	// Initialize firmware
	dxl_initialize(0,1);		//initialize dynamixel communication
	serial_initialize(57600);		//initialize serial communication
	
	//initialize sensors
	sensor_init(SENSOR_FRONT,SENSOR_DISTANCE);
	sensor_init(SENSOR_FRONTLEFT,SENSOR_IR);
	sensor_init(SENSOR_FRONTRIGHT,SENSOR_IR);
	sensor_init(SENSOR_BACKLEFT,SENSOR_IR);
	sensor_init(SENSOR_BACKRIGHT,SENSOR_IR);
	
	//initialize I/O
	io_init();
	io_set_interrupt(BTN_START, &reset_state); //assign callback function when start button is pressed 
	
	// Activate general interrupts
	sei();
	
	// Set motors to wheel mode
	motor_set_mode(254, MOTOR_WHEEL_MODE);
	
	// Set serial communication through ZigBee
	serial_set_zigbee();
	
}

int serial_open(urg_serial_t *serial, const char *device, long baudrate)
{
    // COM10 以降への対応用
    enum { NameLength = 11 };
    char adjusted_device[NameLength];

    serial_initialize(serial);

    /* COM ポートを開く */
    _snprintf(adjusted_device, NameLength, "\\\\.\\%s", device);
    serial->hCom = CreateFileA(adjusted_device, GENERIC_READ | GENERIC_WRITE,
                               0, NULL, OPEN_EXISTING,
                               FILE_ATTRIBUTE_NORMAL, NULL);

    if (serial->hCom == INVALID_HANDLE_VALUE) {
        // !!! store error_message buffer
        //printf("open failed: %s\n", device);
        return -1;
    }

    /* 通信サイズの更新 */
    SetupComm(serial->hCom, 4096 * 8, 4096);

    /* ボーレートの変更 */
    serial_set_baudrate(serial, baudrate);

    /* シリアル制御構造体の初期化 */
    serial->has_last_ch = False;

    /* タイムアウトの設定 */
    serial->current_timeout = 0;
    set_timeout(serial, serial->current_timeout);

    return 0;
}

int serial_connect(serial_t *serial, const char *device, long baudrate)
{

  enum { NameLength = 11 };
  char adjusted_device[NameLength];

  serial_initialize(serial);


  _snprintf(adjusted_device, NameLength, "\\\\.\\%s", device);
  serial->hCom_ = CreateFileA(adjusted_device, GENERIC_READ | GENERIC_WRITE, 0,
                              NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);

  if (serial->hCom_ == INVALID_HANDLE_VALUE) {
    printf("open failed: %s\n", device);
    return -1;
  }


  SetupComm(serial->hCom_, 4096 * 8, 4096);


  serial_setBaudrate(serial, baudrate);


  serial->has_last_ch_ = False;


  serial->current_timeout_ = 0;
  setTimeout(serial, serial->current_timeout_);

  return 0;
}

void sensorInit(void){
printf( "### sensorInit\n");
	
//	DDRA  = 0xFC;
//	PORTA = 0xFC;
	
	serial_initialize(57600);
	sei();

	ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1); // ADC Enable, Clock 1/64div.
	ADMUX = ADC_PORT_1; // ADC Port X Select
}

void
_kernel_call_inirtn(void)
{
	macaddr_init( (VP_INT)(0) );
	application_init( (VP_INT)(0) );
	sdev_init( (VP_INT)(0) );
	stdfile_init( (VP_INT)(0) );
	rtc_init( (VP_INT)(0) );
	mci_init( (VP_INT)(0) );
	dma_init( (VP_INT)(0) );
	timer_initialize( (VP_INT)(0) );
	serial_initialize( (VP_INT)(0) );
}

int main(void)
{
	//int Value = 0;
	
	DDRC  = 0x7F;
	PORTC = 0x7D;
	serial_initialize(57600);				// USART Initialize
	//serial_initialize(250000);				// USART Initialize
	dxl_initialize( 0, 7 ); // Not using device index
	sei();
	SetUpServos();
	SetConstants();
	SetKConstants();

	printf("ARMS CONTROL - Left \r" );
	printf("Ver1.0 - Last Moddification 23-03-15\r" );

	while (1)
	{
	
		
	if(cmdReady)
	{
		 if(!execCommand()) printf("Unknown command\r");
	}
	
	
	if(doLoop)
	{
		executeControl();
		count = count + 100;
	}
	
	if(doGripper)
	{
		executeGripper();
	}
		
		_delay_ms(3);
	

	Leds();

	}

	return 1;
}

void Fable_init(){
		DDRA=0xFF;//PORTA is output

		//serial_initialize(9600);
		serial_initialize(57600);

		//serial_set_rx_callback(&serial_receive_data);
		//Init Timer
//		TCCR0A = (1<<COM0A1)|(1<<COM0A0);  //Set OC0A on Compare Match
//		TCCR0B = (1<<FOC0A)|(1<<CS01)|(1<<CS00); //Force Output Compare A and 64 prescaler
//		OCR0A = 125;	//1ms according to KAVRCalc (8Mhz, 1msm, 64 prescaler, 0.0% error)
//		TIMSK0 = 1<<OCIE0A; 	// Enable Timer 0 Output Compare A Match Event Interrupt

//		TCCR0A = (1<<WGM01);//Clear on compare |(1<<COM0A0);  //Set OC0A on Compare Match
//		TCCR0B = (1<<CS01)|(1<<CS00); //64 prescaler
//		OCR0A = 125;	//1ms -> 8M/2*64*1000
//		TIMSK0 = 1<<OCIE0A; 	// Enable Timer 0 Output Compare A Match Event Interrupt
		start_system_timer();
		accel_init();
		i2c_init(400);// init i2c 100 khz
		ext_uart_init();
		ir_init(EXT_UART_0);
		ir_init(EXT_UART_1);
		ir_init(EXT_UART_2);
		ir_init(EXT_UART_3);

		gyro_set_mode(GYRO_MODE_ON);

		sei();//enable interrupts
		ase_printf("fw initialized\n");
/*
		//ext_adc_init();//only on new board
		ext_uart_enable_int_rx(1,&ext_uart_receive);


		ir_init(EXT_UART_2);
		ir_init(EXT_UART_3);

		gyro_set_mode(GYRO_MODE_ON);// turn on gyroscope

		//dxl_initialize(0,207); //init dinamixel, baud= 57600 (to check)

		//printf("External UART %s\n",i2c_read_byte(0xd8,0x1c)==1?"OK":"NOT OK");//check communication works
*/
}

int main(void)
{
	
	serial_initialize(57600);
	zigbee_initialize();
	sei();
	
	legsInit();
	
	
	while(1)
	{
		move(FRONT_LEFT,300,1,1);
		move(BACK_RIGHT,300,1,1);
	}

	return 0;
}

int main(int argc, char* argv[]) {
    util::ParseInputArgs(argc, argv);

    pointVec_t      points;
    pointVec_t      hull;
    util::my_time_t tm_init, tm_start, tm_end;

    std::cout << "Starting serial version of QUICK HULL algorithm" << std::endl;

    tm_init = util::gettime();
    serial_initialize(points);
    tm_start = util::gettime();
    std::cout << "Init time: " << util::time_diff(tm_init, tm_start) << "  Points in input: " << points.size() << "\n";
    tm_start = util::gettime();
    quickhull(points, hull);
    tm_end = util::gettime();
    std::cout << "Serial time: " << util::time_diff(tm_start, tm_end) << "  Points in hull: " << hull.size() << "\n";
}

int main(){
	//portInit();
	serial_initialize(57600);
	dxl_initialize( 0, 1 ); // init with baud = 1 Mbps
	ADCInit();
	sei(); //Enables global interrupts

	unsigned int distanceLeft, distanceRight, front, movingLeft, movingRight;
	signed int speedLeft, speedRight;
	while(1) {
		//printf("%d   %d\n\n",getSensorValue(3),getSensorValue(4));
		//_delay_ms(1000);

		// Get sensor reading (in cm)
		front = DMSDistance(getSensorValue(1));
		distanceLeft = IRValue(getSensorValue(3));
		distanceRight = IRValue(getSensorValue(4));

		// Calculating the required speed
		speedRight = (int)((front-40*distanceLeft));
		speedLeft = (int)((front-40*distanceRight));

		movingLeft = dxl_read_byte( 1, 38 );
		movingRight = dxl_read_byte(2,38);

		if(movingLeft < 15 && movingRight < 15){
			wheel(1,0);
			wheel(2,-20);
			_delay_ms(1000);
		}else{
			wheel(1,-speedRight);
			wheel(2,speedLeft);
		}


		printf("%d   %d  -  %d\n\n",speedLeft,speedRight,getSensorValue(1));
		//_delay_ms(1000);
		// Making the wheels spin


	}

	return 0;
}

/*
 打开串口
 */
int serial_open(urg_serial_t *serial, const char *device, long baudrate)
{
    int flags = 0;
    int ret = 0;

    serial_initialize(serial);

#ifndef URG_MAC_OS
    enum { O_EXLOCK = 0x0 }; /*  Not used in Linux, used as dummy */
#endif
	/* Linux系统打开一个设备，详细的可以看 UNIX高级环境编程 */
    serial->fd = open(device, O_RDWR | O_EXLOCK | O_NONBLOCK | O_NOCTTY);
    if (serial->fd < 0) {
        /*  设备打开失败 */
        //strerror_r(errno, serial->error_string, ERROR_MESSAGE_SIZE);
        return -1;
    }

    flags = fcntl(serial->fd, F_GETFL, 0);
    fcntl(serial->fd, F_SETFL, flags & ~O_NONBLOCK);

    /*  串口初始化部分  */
    tcgetattr(serial->fd, &serial->sio);
    serial->sio.c_iflag = 0;
    serial->sio.c_oflag = 0;
	/* 下面的大写字母表示的都是Linux系统的宏定义，没有必要全部搞懂，看一下了解一下 */
    serial->sio.c_cflag &= ~(CSIZE | PARENB | CSTOPB);
    serial->sio.c_cflag |= CS8 | CREAD | CLOCAL;
    serial->sio.c_lflag &= ~(ICANON | ECHO | ISIG | IEXTEN);

    serial->sio.c_cc[VMIN] = 0;
    serial->sio.c_cc[VTIME] = 0;
	/* 串口设置波特率 */
    ret = serial_set_baudrate(serial, baudrate);
    if (ret < 0) {
        return ret;
    }

    /*  Initializes serial control structures */
    serial->has_last_ch = False;

    return 0;
}

static void __init mv_init(void)
{
        /* init the Board environment */
       	mvBoardEnvInit();

        /* init the controller environment */
        if (mvCtrlEnvInit() ) {
            printk( "Controller env initialization failed.\n" );
            return;
        }


	/* Init the CPU windows setting and the access protection windows. */
	if (mvCpuIfInit(mv_sys_map())) {

		printk( "Cpu Interface initialization failed.\n" );
		return;
	}
#if defined (CONFIG_MV78XX0_Z0)
	mvCpuIfBridgeReorderWAInit();
#endif

    	/* Init Tclk & SysClk */
    	mvTclk = mvBoardTclkGet();
   	mvSysclk = mvBoardSysClkGet();
	
        support_wait_for_interrupt = 1;
  
#ifdef CONFIG_JTAG_DEBUG
            support_wait_for_interrupt = 0; /*  for Lauterbach */
#endif
	mv_vfp_init();	
	elf_hwcap &= ~HWCAP_JAVA;

   	serial_initialize();

	/* At this point, the CPU windows are configured according to default definitions in mvSysHwConfig.h */
	/* and cpuAddrWinMap table in mvCpuIf.c. Now it's time to change defaults for each platform.         */
	mvCpuIfAddrDecShow(whoAmI());

    	print_board_info();
}

/* �ڑ� */
int serial_connect(serial_t *serial, const char *device, long baudrate)
{
  int flags = 0;
  int ret = 0;

  serial_initialize(serial);

#ifndef MAC_OS
  enum { O_EXLOCK = 0x0 }; /* Linux �ł͎g���Ȃ��̂Ń_�~�[���쐬���Ă��� */
#endif
  serial->fd_ = open(device, O_RDWR | O_EXLOCK | O_NONBLOCK | O_NOCTTY);
  if (serial->fd_ < 0) {
    /* �ڑ��Ɏ��s */
    strerror_r(errno, serial->error_string_, SerialErrorStringSize);
    return SerialConnectionFail;
  }

  flags = fcntl(serial->fd_, F_GETFL, 0);
  fcntl(serial->fd_, F_SETFL, flags & ~O_NONBLOCK);

  /* �V���A���ʐM�̏��� */
  tcgetattr(serial->fd_, &serial->sio_);
  serial->sio_.c_iflag = 0;
  serial->sio_.c_oflag = 0;
  serial->sio_.c_cflag &= ~(CSIZE | PARENB | CSTOPB);
  serial->sio_.c_cflag |= CS8 | CREAD | CLOCAL;
  serial->sio_.c_lflag &= ~(ICANON | ECHO | ISIG | IEXTEN);

  serial->sio_.c_cc[VMIN] = 0;
  serial->sio_.c_cc[VTIME] = 0;

  /* �{�[���[�g�̕ύX */
  ret = serial_setBaudrate(serial, baudrate);
  if (ret < 0) {
    return ret;
  }

  /* �V���A������\���̂̏��� */
  serial->has_last_ch_ = False;

  return 0;
}

int serial_open(urg_serial_t *serial, const char *device, long baudrate)
{
    int flags = 0;
    int ret = 0;

    serial_initialize(serial);

#ifndef URG_MAC_OS
    enum { O_EXLOCK = 0x0 }; /*  Not used in Linux, used as dummy */
#endif
    serial->fd = open(device, O_RDWR | O_EXLOCK | O_NONBLOCK | O_NOCTTY);
    if (serial->fd < 0) {
        /*  Connection failed */
        //strerror_r(errno, serial->error_string, ERROR_MESSAGE_SIZE);
        return -1;
    }

    flags = fcntl(serial->fd, F_GETFL, 0);
    fcntl(serial->fd, F_SETFL, flags & ~O_NONBLOCK);

    /*  Initializes serial communication  */
    tcgetattr(serial->fd, &serial->sio);
    serial->sio.c_iflag = 0;
    serial->sio.c_oflag = 0;
    serial->sio.c_cflag &= ~(CSIZE | PARENB | CSTOPB);
    serial->sio.c_cflag |= CS8 | CREAD | CLOCAL;
    serial->sio.c_lflag &= ~(ICANON | ECHO | ISIG | IEXTEN);

    serial->sio.c_cc[VMIN] = 0;
    serial->sio.c_cc[VTIME] = 0;

    ret = serial_set_baudrate(serial, baudrate);
    if (ret < 0) {
        return ret;
    }

    /*  Initializes serial control structures */
    serial->has_last_ch = False;

    return 0;
}

int main(void)
{
	int Value = 0;

	serial_initialize(57600);				// USART Initialize
	sei();

	printf( "\n\nSerial Comm. example for CM-510\n\n" );

	while (1)
	{
		unsigned char ReceivedData = getchar();
		if(ReceivedData == 'u')
			Value++;
		else if(ReceivedData == 'd')
			Value--;

		printf("%d\r\n", Value);
	}
	return 1;
}

void urg_initialize(urg_t *urg)
{
  serial_initialize(&urg->serial_);
  urg->errno_ = UrgNoError;
  urg->last_timestamp_ = 0;
}