Exemplo n.º 1
0
/*
 *	メインタスク
 */
void main_task(VP_INT exinf)
{
	int len;
	char buf[256];
	const char cmnd[] = "RVAL\r";
	unsigned char chr;

	vmsk_log(LOG_UPTO(LOG_INFO), LOG_UPTO(LOG_EMERG));
	syslog(LOG_NOTICE, "Sample program starts (exinf = %d).", (INT) exinf);

	Initialize();

	while(1)
	{
		SIO_FlashBuffer();

		strcpy(buf, cmnd);
		len = SIO_Write((unsigned char*)buf, sizeof(cmnd));			// Send Command

		chr = 0;
		while(chr != '\r')
		{
			len = SIO_ReadForever((unsigned char*)buf, 1);
			chr = buf[0];
		tslp_tsk(100);
		}

		tslp_tsk(100);
	}

	syslog(LOG_NOTICE, "Sample program ends.");
	kernel_exit();
}
Exemplo n.º 2
0
/*
 *  メインタスク
 */
void main_task(intptr_t exinf)
{

	syslog(LOG_NOTICE, "Sample program starts (exinf = %d).", (int_t) exinf);

		/* GPIO0のLED制御ピンを出力にする */
	GPIO_SetDir(
			0,			// GPIOポート番号
			LEDMASK,	// 操作するビットマスク
			1			// 設定する値
			);

	while(1)
	{
			/* 500m秒待つ */
		SVC_PERROR(tslp_tsk(500));
			/* LED制御ピンをLにする */
		GPIO_ClearValue(
				0,			// GPIOポート番号
				LEDMASK	// 操作するビットマスク
				);
			/* 500m秒待つ */
		SVC_PERROR(tslp_tsk(500));
			/* LED制御ピンをHにする */
		GPIO_SetValue(
				0,			// GPIOポート番号
				LEDMASK	// 操作するビットマスク
				);
	}


	syslog(LOG_NOTICE, "Sample program ends.");
	SVC_PERROR(ext_ker());
	assert(0);
}
Exemplo n.º 3
0
static void debug_i2c_sensor() {
	// Use port 2 for debug
	//float_inport_2();
#if 1
    IicPortEnable(1);
#else
	devcon.Connection[1] = CONN_NXT_IIC;
	devcon.Mode[1] = 255;
	Device1Ioctl(NULL, NULL, IIC_SET_CONN, (unsigned long)&devcon);
	while(((volatile IICPORT*)(IicPort+1))->Initialised == 0);
#endif
#if 0
	syslog(LOG_NOTICE, "IIC_SET_CONN");
	devcon.Mode[1] = 1;
	Device1Ioctl(NULL, NULL, IIC_SET_CONN, (unsigned long)&devcon);
#endif

	// I2C
	while (0) {
	int Port = 1;
	UBYTE   TmpBuffer[IIC_DATA_LENGTH];
    IicPort[Port].OutBuffer[0]  =  IicPort[Port].Addr;
    IicPort[Port].OutBuffer[1]  =  0x42; //0x08;
    IicPort[Port].OutBuffer[2]  =  0x00;
    IicPort[Port].OutBuffer[3]  =  0x00;
    IicPort[Port].OutLength     =  2;
    IicPort[Port].InLength      =  8;
    IicPortSend(Port);
    tslp_tsk(1000);
    IicPortReceive(Port, TmpBuffer);
    syslog(LOG_EMERG, "TmpBuffer %d %d %d %d %d", TmpBuffer[0], TmpBuffer[1], TmpBuffer[2], TmpBuffer[3], TmpBuffer[4]);

	}

#if 0
    while (1) {
    	IicPortReceive(Port, TmpBuffer);
    	tslp_tsk(1000);
    	target_fput_log('l');
    }
#endif

	T_CCYC ccyc;
	ccyc.cycatr = TA_STA;
	ccyc.cychdr = debug_i2c_cyc;
	ccyc.cycphs = 0;
	ccyc.cyctim = 200;
	//ER_ID ercd = acre_cyc(&ccyc);
	//assert(ercd > 0);
}
Exemplo n.º 4
0
int
nxtrike_sound(int hz, int msec, int volume){
    ev3_speaker_set_volume(volume);
    ev3_speaker_play_tone(hz, msec);
    tslp_tsk(msec);
	return 0;
}
Exemplo n.º 5
0
void test_ev3_cychdr(intptr_t idx) {
    ev3_lcd_fill_rect(0, fonth * idx, EV3_LCD_WIDTH, fonth, EV3_LCD_WHITE);
    char buf[100];
    sprintf(buf, "EV3CYC %d count %d", idx, ++count[idx]);
    ev3_lcd_draw_string(buf, 0, fonth * idx);
	SVC_PERROR(tslp_tsk(10));
}
Exemplo n.º 6
0
void bluetooth_task(intptr_t unused) {
#if defined(DEBUG)
	syslog(LOG_DEBUG, "[bluetooth] Start main task.");
#endif

    run_loop_init(RUN_LOOP_EMBEDDED);

    // Initialize HCI
    bt_control_t             *control   = bt_control_cc256x_instance();
	hci_transport_t          *transport = hci_transport_h4_dma_instance();
	hci_uart_config_t        *config    = hci_uart_config_cc256x_instance();
	const remote_device_db_t *db        = &remote_device_db_memory;
	hci_init(transport, config, control, db);

    // Initialize SPP (Serial Port Profile)
    bluetooth_spp_initialize();

	// Power on
	bt_control_cc256x_enable_ehcill(false);
	hci_power_control(HCI_POWER_ON);

    run_loop_execute();
#if 0 // Code for debugging
        while(1) {
//        	bluetooth_uart_isr();
        	embedded_execute_once();
//        			if(rx_size > 0 && (UART2.IIR_FCR & 0x4)) // TODO: dirty hack
//        				AINTC.SISR = UART2_INT;
        	tslp_tsk(1); // TODO: Use interrupt instead of sleeping. -- ertl-liyixiao
        }
#endif
}
Exemplo n.º 7
0
void
task2(intptr_t exinf)
{
	ER_UINT	ercd;

	switch (++task2_count) {
	case 1:
		check_point(2);
		ercd = slp_tsk();
		check_ercd(ercd, E_NOSPT);

		ercd = tslp_tsk(TEST_TIME_PROC);
		check_ercd(ercd, E_NOSPT);

		ercd = dly_tsk(TEST_TIME_PROC);
		check_ercd(ercd, E_NOSPT);

		ercd = wai_sem(SEM1);
		check_ercd(ercd, E_NOSPT);

		ercd = pol_sem(SEM1);
		check_ercd(ercd, E_OK);

		ercd = pol_sem(SEM1);
		check_ercd(ercd, E_TMOUT);

		ercd = twai_sem(SEM1, TEST_TIME_PROC);
		check_ercd(ercd, E_NOSPT);

		ercd = ext_tsk();
		check_ercd(ercd, E_OK);

		check_point(0);

	case 2:
		check_point(6);
		ercd = rot_rdq(MID_PRIORITY);
		check_ercd(ercd, E_NOSPT);

		ercd = ext_tsk();
		check_ercd(ercd, E_OK);

		check_point(0);

	case 3:
		check_point(10);
		ercd = rot_rdq(MID_PRIORITY);
		check_ercd(ercd, E_OK);

		ercd = ext_tsk();
		check_ercd(ercd, E_OK);

		check_point(0);

	default:
		check_point(0);
	}
	check_point(0);
}
Exemplo n.º 8
0
/*
 *  タッチセンサハンドラ
 */
void nxtrike_touch_sensor_handler(intptr_t exinf)
{
	//nxtrike_touch_sensor_value ^= (sensor_adc(TOUCH_SENSOR) < 512);
    while (1) {
    nxtrike_touch_sensor_value = ev3_touch_sensor_is_pressed(touch_sensor);
    SVC_PERROR(tslp_tsk(NXTRIKE_TOUCH_SENSOR_HANDLER_T));
    }
}
Exemplo n.º 9
0
/*
 *  並行実行されるタスク
 */
void task(VP_INT exinf)
{
	_toppers_cxxrt_reset_specific();

	volatile UW	i;
	INT	n = 0;
	INT	tskno = (INT) exinf;
	char	*graph[] = { "|", "  +", "    *" };
	char	c;

	ena_tex();
	while (1) {
		syslog(LOG_NOTICE, "task%d is running (%03d).   %s",
					tskno, ++n, graph[tskno-1]);
		for (i = 0; i < task_loop; i++);
		c = message[tskno-1];
		message[tskno-1] = 0;
		switch (c) {
		case 'e':
			syslog(LOG_INFO, "#%d#ext_tsk()", tskno);
			ext_tsk();
		case 's':
			syslog(LOG_INFO, "#%d#slp_tsk()", tskno);
			syscall(slp_tsk());
			break;
		case 'S':
			syslog(LOG_INFO, "#%d#tslp_tsk(10000)", tskno);
			syscall(tslp_tsk(10000));
			break;
		case 'd':
			syslog(LOG_INFO, "#%d#dly_tsk(10000)", tskno);
			syscall(dly_tsk(10000));
			break;
		case 'y':
			syslog(LOG_INFO, "#%d#dis_tex()", tskno);
			syscall(dis_tex());
			break;
		case 'Y':
			syslog(LOG_INFO, "#%d#ena_tex()", tskno);
			syscall(ena_tex());
			break;
#ifdef CPUEXC1
		case 'z':
			syslog(LOG_NOTICE, "#%d#raise CPU exception", tskno);
			RAISE_CPU_EXCEPTION;
			break;
		case 'Z':
			loc_cpu();
			syslog(LOG_NOTICE, "#%d#raise CPU exception", tskno);
			RAISE_CPU_EXCEPTION;
			unl_cpu();
			break;
#endif /* CPUEXC1 */
		default:
			break;
		}
	}
}
Exemplo n.º 10
0
/*
 * 駆動ハンドラ
 * 機能: 駆動タスクを起床する
 */
void
nxtrike_drive_handler(intptr_t exinf)
{
	//iwup_tsk(NXTRIKE_DRIVE_TASK);
	while (1) {
    SVC_PERROR(wup_tsk(NXTRIKE_DRIVE_TASK));
    SVC_PERROR(tslp_tsk(NXTRIKE_DRIVE_HANDLER_T));
    }
}
Exemplo n.º 11
0
/*
 *  並行実行されるタスク
 */
void task(intptr_t exinf)
{
	volatile ulong_t	i;
	int_t		n = 0;
	int_t		tskno = (int_t) exinf;
	const char	*graph[] = { "|", "  +", "    *" };
	char		c;

	SVC_PERROR(ena_tex());
	while (true) {
		syslog(LOG_NOTICE, "task%d is running (%03d).   %s",
										tskno, ++n, graph[tskno-1]);
		for (i = 0; i < task_loop; i++);
		c = message[tskno-1];
		message[tskno-1] = 0;
		switch (c) {
		case 'e':
			syslog(LOG_INFO, "#%d#ext_tsk()", tskno);
			SVC_PERROR(ext_tsk());
			assert(0);
		case 's':
			syslog(LOG_INFO, "#%d#slp_tsk()", tskno);
			SVC_PERROR(slp_tsk());
			break;
		case 'S':
			syslog(LOG_INFO, "#%d#tslp_tsk(10000)", tskno);
			SVC_PERROR(tslp_tsk(10000));
			break;
		case 'd':
			syslog(LOG_INFO, "#%d#dly_tsk(10000)", tskno);
			SVC_PERROR(dly_tsk(10000));
			break;
		case 'y':
			syslog(LOG_INFO, "#%d#dis_tex()", tskno);
			SVC_PERROR(dis_tex());
			break;
		case 'Y':
			syslog(LOG_INFO, "#%d#ena_tex()", tskno);
			SVC_PERROR(ena_tex());
			break;
#ifdef CPUEXC1
		case 'z':
			syslog(LOG_NOTICE, "#%d#raise CPU exception", tskno);
			RAISE_CPU_EXCEPTION;
			break;
		case 'Z':
			SVC_PERROR(loc_cpu());
			syslog(LOG_NOTICE, "#%d#raise CPU exception", tskno);
			RAISE_CPU_EXCEPTION;
			SVC_PERROR(unl_cpu());
			break;
#endif /* CPUEXC1 */
		default:
			break;
		}
	}
}
Exemplo n.º 12
0
/*
 *  並行実行されるタスク
 */
void task(VP_INT exinf)
{
	volatile UW	i;
	INT		n = 0;
	INT		tskno = (INT) exinf;
	const char	*graph[] = { "|", "  +", "    *" };
	char		c;

	ena_tex();
	while (1) {
		syslog(LOG_NOTICE, "task%d is running (%03d).   %s",
					tskno, ++n, graph[tskno-1]);
		for (i = 0; i < task_loop; i++);
		c = message[tskno-1];
		message[tskno-1] = 0;
		switch (c) {
		case 'e':
			syslog(LOG_INFO, "#%d#ext_tsk()", tskno);
			ext_tsk();
		case 's':
			syslog(LOG_INFO, "#%d#slp_tsk()", tskno);
			syscall(slp_tsk());
			break;
		case 'S':
			syslog(LOG_INFO, "#%d#tslp_tsk(10000)", tskno);
			syscall(tslp_tsk(10000));
			break;
		case 'd':
			syslog(LOG_INFO, "#%d#dly_tsk(10000)", tskno);
			syscall(dly_tsk(10000));
			break;
		case 'y':
			syslog(LOG_INFO, "#%d#dis_tex()", tskno);
			syscall(dis_tex());
			break;
		case 'Y':
			syslog(LOG_INFO, "#%d#ena_tex()", tskno);
			syscall(ena_tex());
			break;
#ifdef CPUEXC1
		case 'z':
			syslog(LOG_NOTICE, "#%d#raise CPU exception", tskno);
			RAISE_CPU_EXCEPTION;
			break;
#endif /* CPUEXC1 */
		case 'Z':
			syslog(LOG_NOTICE, "Sample program ends with exception.");
			kernel_exit();
			break;
		default:
			break;
		}
	}
}
Exemplo n.º 13
0
static mrb_value
mrb_mruby_task_sleep(mrb_state *mrb, mrb_value self)
{
	mrb_int timeout;
	mrb_int ret = mrb_get_args(mrb, "|i", &timeout);
	if(ret == 0){
		slp_tsk();
	}else{
		tslp_tsk(timeout);
	}
	return self;
}
Exemplo n.º 14
0
void
task2(intptr_t exinf)
{
	ER_UINT	ercd;

	switch (++task2_count) {
	case 1:
		check_point(2);
		ercd = slp_tsk();
		check_ercd(ercd, E_NOSPT);

		check_point(3);
		ercd = tslp_tsk(10);
		check_ercd(ercd, E_NOSPT);

		check_point(4);
		ercd = dly_tsk(10);
		check_ercd(ercd, E_NOSPT);

		check_point(5);
		ercd = ext_tsk();
		check_ercd(ercd, E_OK);

		check_point(0);

	case 2:
		check_point(23);
		ercd = rot_rdq(MID_PRIORITY);
		check_ercd(ercd, E_NOSPT);

		check_point(24);
		ercd = ext_tsk();
		check_ercd(ercd, E_OK);

		check_point(0);

	case 3:
		check_point(30);
		ercd = rot_rdq(MID_PRIORITY);
		check_ercd(ercd, E_OK);

		check_point(31);
		ercd = ext_tsk();
		check_ercd(ercd, E_OK);

		check_point(0);

	default:
		check_point(0);
	}
	check_point(0);
}
Exemplo n.º 15
0
ER load_application(const void *mod_data, SIZE mod_data_sz) {
	ER ercd;

//	ev3_led_set_color(LED_GREEN);
//	chg_status(STATUS_RUNNING);

#if 0
	// Wait for pressing center button
    SYSTIM time = 0;
    uint32_t ledcolor = 0;
	syslog(LOG_NOTICE, "Press center button to run the application.");
	while(!global_brick_info.button_pressed[BRICK_BUTTON_ENTER]) {
		SYSTIM newtime;
		get_tim(&newtime);
		if (newtime - time > 500) { // Blink LED
			brick_misc_command(MISCCMD_SET_LED, ledcolor);
			ledcolor ^= TA_LED_RED | TA_LED_GREEN;
			time = newtime;
		}
	}
	while(global_brick_info.button_pressed[BRICK_BUTTON_ENTER]) {
		SYSTIM newtime;
		get_tim(&newtime);
		if (newtime - time > 500) { // Blink LED
			brick_misc_command(MISCCMD_SET_LED, ledcolor);
			ledcolor ^= TA_LED_RED | TA_LED_GREEN;
			time = newtime;
		}
	}
#endif

	brick_misc_command(MISCCMD_SET_LED, TA_LED_GREEN);
	platform_soft_reset();
	ercd = dmloader_ins_ldm(mod_data, mod_data_sz, 1);
	if (ercd != E_OK) {
		syslog(LOG_ERROR, "Failed to load application, ercd: %d", ercd);
	} else {
		app_loaded = true;
		SVC_PERROR(sta_alm(APP_TER_BTN_ALM, 0));
		SVC_PERROR(wai_sem(APP_TER_SEM));
		syslog(LOG_NOTICE, "Terminate application.");
		SVC_PERROR(dmloader_rmv_ldm(1));
		brick_misc_command(MISCCMD_SET_LED, TA_LED_GREEN);
	}
//	chg_status(STATUS_IDLE);
	platform_soft_reset();
	tslp_tsk(500);

	return ercd;
}
Exemplo n.º 16
0
static int __ui_tslp_tsk(struct pt_regs *regs)
{
	TMO tmout = __xn_reg_arg1(regs);
	ER err;

	err = tslp_tsk(tmout);

	if (err == E_RLWAI) {
		uitask_t *task = ui_current_task();
		if (!xnthread_test_info(&task->threadbase, uITRON_TASK_RLWAIT))
			err = -EINTR;
	}

	return err;
}
Exemplo n.º 17
0
void
tcp_echo_srv_task(intptr_t exinf)
{
	ID	tskid;
	ER	error;

	syscall(get_tid(&tskid));
	syslog(LOG_NOTICE, "[TCP ECHO SRV:%d,%d] started.", tskid, (ID)exinf);
	while (true) {
		while ((error = tcp_echo_srv((ID)exinf, TCP_ECHO_SRV_REPID)) == E_OK)
			;
		syslog(LOG_NOTICE, "[TES:%02d] goto sleep 60[s], error: %s", (ID)exinf, itron_strerror(error));
		tslp_tsk(60 * 1000);
		}
	}
Exemplo n.º 18
0
void initialize_mmcsd() {

    /* Configure EDMA to service the MMCSD events. */
    MMCSDEdmaInit();

#if 0 // TODO: Check if these steps are necessary? -- ertl-liyixiao
    /* Perform pin-mux for MMCSD pins. */
    MMCSDPinMuxSetup();

    /* Enable module clock for MMCSD. */
    MMCSDModuleClkConfig();

    DelayTimerSetup();
#endif

    /* Basic controller initializations */
    MMCSDControllerSetup();

    /* Initialize the MMCSD controller */
    MMCSDCtrlInit(&ctrlInfo);

    MMCSDIntEnable(&ctrlInfo);

    /* Check SD card status */
    while(1) {
    	if((MMCSDCardPresent(&ctrlInfo)) == 1) {
    		syslog(LOG_DEBUG, "Card is present");
    		break;
    	}
    	syslog(LOG_ERROR, "Card is not present");
    	tslp_tsk(1000);
    }

    /* Initialize device control interface for FatFS */
    diskio_initialize(&sdCard);

//    static char tmpBuf[1024] __attribute__ ((aligned (SOC_EDMA3_ALIGN_SIZE)));
//    MMCSDReadCmdSend(&ctrlInfo, tmpBuf, 0, 1);
//    MMCSDReadCmdSend(&ctrlInfo, tmpBuf, 0, 1);
//    MMCSDWriteCmdSend(&ctrlInfo, tmpBuf, 0, 1);
//    MMCSDWriteCmdSend(&ctrlInfo, tmpBuf, 0, 1);
//    dump_mmc(&MMCSD0);
//    while(1) tslp_tsk(1000);
}
Exemplo n.º 19
0
void load_bootapp() {
	// Open
	static FIL fil;
	FRESULT res = f_open(&fil, BOOTAPP_PATH, FA_READ);
	if (res == FR_NO_FILE) return; // 'bootapp' doesn't exist, do nothing.
	if (res != FR_OK) {
		syslog(LOG_ERROR, "Open boot application '%s' failed, FRESULT: %d", BOOTAPP_PATH, res);
		return;
	}
	syslog(LOG_NOTICE, "Found boot application '%s', start to load.", BOOTAPP_PATH);

	// Read
	char *ptr_c = (void*)app_binary_buf;
	while (1) {
		UINT br;
		res = f_read(&fil, ptr_c, 1, &br);
		assert(res == FR_OK);
		if (br > 0)
			ptr_c += br;
		else
			break;
	}
	res = f_close(&fil);
	assert(res == FR_OK);


#if 0
	FILE *fin = fopen(filepath, "rb");
	while (fread(ptr_c, 1, 1, fin) > 0)
	ptr_c++;
	fclose(fin);
#endif

	SIZE filesz = ptr_c - (char*) app_binary_buf;
	syslog(LOG_NOTICE, "Loading file completed, file size: %d.", filesz);

	ER ercd = load_application(app_binary_buf, filesz);
	if (ercd != E_OK) {
		syslog(LOG_NOTICE, "Load application failed, ercd: %d.", ercd);
		tslp_tsk(500);
	}
}
Exemplo n.º 20
0
void
task2(intptr_t exinf)
{
	ER		ercd;

	check_point(24);
	ercd = tslp_tsk(10);
	check_ercd(ercd, E_TMOUT);

	check_point(27);
	ercd = sta_alm(ALM1, 10);
	check_ercd(ercd, E_OK);

	wait_var();

	check_point(29);
	ercd = ext_tsk();

	check_point(0);
}
Exemplo n.º 21
0
/*
 *  並行実行されるタスク
 */
void task(intptr_t exinf)
{
	volatile ulong_t	i;
	int_t		n = 0;
	int_t		tskno = (int_t) (exinf);
	const char	*graph[] = { "|", "  +", "    *", "||", "  ++", "    **" };
	char_t		c;
	  
	while (1) {
		syslog(LOG_NOTICE, "task%d is running (%03d).   %s",
			   tskno, ++n, graph[tskno-1]);
		for (i = 0; i < task_loop; i++);
		c = message[tskno-1];
		message[tskno-1] = 0;
		switch (c) {
		case 'e':
			syslog(LOG_INFO, "#%d#ext_tsk()", tskno);
			SVC_PERROR(ext_tsk());
			assert(0);
		case 's':
			syslog(LOG_INFO, "#%d#slp_tsk()", tskno);
			SVC_PERROR(slp_tsk());
			break;
		case 'S':
			syslog(LOG_INFO, "#%d#tslp_tsk(10000)", tskno);
			SVC_PERROR(tslp_tsk(10000));
			break;
		case 'd':
			syslog(LOG_INFO, "#%d#dly_tsk(10000)", tskno);
			SVC_PERROR(dly_tsk(10000));
			break;
            	case 'g':
			syslog(LOG_INFO, "#%d#mig_tsk(0,%d)", tskno,tsk_mig_prc);
			SVC_PERROR(mig_tsk(0,tsk_mig_prc));
			break;
		default:
			break;
		}
	}
}
void task_led(intptr_t exinf)
{
    uint16_t msg;
    led_init();
    while(1)
    {
        if (rcv_dtq(DTQ_LED, (intptr_t *)&msg) == E_OK) {
            switch (MSG_TARGET(msg)) {
                case DBLED0:
                    led_debug_write(0, MSG_CONTROL(msg));
                    break;
                case DBLED1:
                    led_debug_write(1, MSG_CONTROL(msg));
                    break;
                case DBLED2:
                    led_debug_write(2, MSG_CONTROL(msg));
                    break;
                case DBLED3:
                    led_debug_write(3, MSG_CONTROL(msg));
                    break;
                case SWLED0:
                    led_switch_write(0, MSG_CONTROL(msg));
                    break;
                case SWLED1:
                    led_switch_write(1, MSG_CONTROL(msg));
                    break;
                case SWLED2:
                    led_switch_write(2, MSG_CONTROL(msg));
                    break;
                case SWLED3:
                    led_switch_write(3, MSG_CONTROL(msg));
                    break;
            }
        }
        tslp_tsk(10);
    }
}
Exemplo n.º 23
0
void main_task(intptr_t unused) {

    bt = ev3_serial_open_file(EV3_SERIAL_BT);

	int heartBeatCount = 0;
	Linetrace* linetrace;

	Temp::init();
	tailControl->SetRefValue(90);
	wheelControl->Init();

	while(1) {

		if(1250 < heartBeatCount) {
			linetrace->exec();
		}
		tailControl->Control();
		wheelControl->Control();

		if(heartBeatCount%250 == 0) {
			ev3_led_set_color(LED_ORANGE);
		}
		else if((heartBeatCount+125)%250 == 0) {
			ev3_led_set_color(LED_GREEN);
		}
		heartBeatCount++;

		if(heartBeatCount == 1250) {
			tailControl->SetRefValue(0);
			linetrace = new Linetrace(wheelControl, calibration);

		}

		tslp_tsk(4);
	}

}
Exemplo n.º 24
0
void lcd_refresh_tsk(intptr_t unused) {
	struct st7586fb_par *par = ((struct fb_info *)(spidev.drvdata))->par;

	while (1) {
#if 0 // For test
		struct fb_info *info = spidev.drvdata;
		u8 *vmem = info->screen_base;
		static int i = 0;
		vmem[i++] = 0xFF;
		if (i == (WIDTH+2)/3*HEIGHT) i = 0;
#endif
		st7586_set_addr_win(par, 0, 0, WIDTH, HEIGHT);
		st7586_write_cmd(par, ST7586_RAMWR);

		/* Blast frame buffer to ST7586 internal display RAM */
		ER ercd = st7586_write_data_buf(par, on_display_fb->pixels, (WIDTH + 2) / 3 * HEIGHT);
		assert(ercd == E_OK);

#if 0 // Legacy code
		st7586fb_deferred_io(spidev.drvdata, NULL);
#endif
		tslp_tsk(1000 / (LCD_FRAME_RATE));
	}
}
Exemplo n.º 25
0
void RTOS::waiTask(uint16 rv_timeout_cnt){
    tslp_tsk(rv_timeout_cnt);
}
Exemplo n.º 26
0
void main_task(intptr_t unused) {
    ev3_lcd_set_font(EV3_FONT_MEDIUM);
    ev3_font_get_size(EV3_FONT_MEDIUM, &fontw, &fonth);

    memfile_t memfile;
    ev3_memfile_load("/test.bmp", &memfile);
    image_t image;
    ev3_image_load(&memfile, &image);
    ev3_lcd_draw_image(&image, 0, fonth * 2);

    //ev3_sta_cyc(TEST_EV3_CYC1);
    // Enable TEST_EV3_CYC2 for 5 seconds
    ev3_sta_cyc(TEST_EV3_CYC2);
    tslp_tsk(5000);
    ev3_stp_cyc(TEST_EV3_CYC2);
#if 0
    // Register button handlers
    ev3_button_set_on_clicked(BACK_BUTTON, button_clicked_handler, BACK_BUTTON);
    ev3_button_set_on_clicked(ENTER_BUTTON, button_clicked_handler, ENTER_BUTTON);
    ev3_button_set_on_clicked(LEFT_BUTTON, button_clicked_handler, LEFT_BUTTON);

    // Configure sensors
    ev3_sensor_config(gyro_sensor, GYRO_SENSOR);

    // Configure motors
    ev3_motor_config(left_motor, LARGE_MOTOR);
    ev3_motor_config(right_motor, LARGE_MOTOR);

    // Start task for self-balancing
    act_tsk(BALANCE_TASK);

    // Open Bluetooth file
    bt = ev3_serial_open_file(EV3_SERIAL_BT);
    assert(bt != NULL);

    // Start task for printing message while idle
	act_tsk(IDLE_TASK);

    while(1) {
    	uint8_t c = fgetc(bt);
    	sus_tsk(IDLE_TASK);
    	switch(c) {
    	case 'w':
    		if(motor_control_drive < 0)
    			motor_control_drive = 0;
    		else
    			motor_control_drive += 10;
    		fprintf(bt, "motor_control_drive: %d\n", motor_control_drive);
    		break;

    	case 's':
    		if(motor_control_drive > 0)
    			motor_control_drive = 0;
    		else
    			motor_control_drive -= 10;
    		fprintf(bt, "motor_control_drive: %d\n", motor_control_drive);
    		break;

    	case 'a':
    		if(motor_control_steer < 0)
    			motor_control_steer = 0;
    		else
    			motor_control_steer += 10;
    		fprintf(bt, "motor_control_steer: %d\n", motor_control_steer);
    		break;

    	case 'd':
    		if(motor_control_steer > 0)
    			motor_control_steer = 0;
    		else
    			motor_control_steer -= 10;
    		fprintf(bt, "motor_control_steer: %d\n", motor_control_steer);
    		break;

    	case 'h':
    		fprintf(bt, "==========================\n");
    		fprintf(bt, "Usage:\n");
    		fprintf(bt, "Press 'w' to speed up\n");
    		fprintf(bt, "Press 's' to speed down\n");
    		fprintf(bt, "Press 'a' to turn left\n");
    		fprintf(bt, "Press 'd' to turn right\n");
    		fprintf(bt, "Press 'i' for idle task\n");
    		fprintf(bt, "Press 'h' for this message\n");
    		fprintf(bt, "==========================\n");
    		break;

    	case 'i':
    		fprintf(bt, "Idle task started.\n");
    		rsm_tsk(IDLE_TASK);
    		break;

    	default:
    		fprintf(bt, "Unknown key '%c' pressed.\n", c);
    	}
    }
#endif
}
Exemplo n.º 27
0
/*
 *  並行実行されるタスク
 */
void task(intptr_t exinf)
{
	volatile ulong_t	i;
	int_t		n = 0;
	int_t		tskno = (int_t) exinf;
	const char	*graph[] = { "|    ", "  +  ", "    *" };
	char		c;
#ifdef TOPPERS_SUPPORT_OVRHDR
	T_ROVR		pk_rovr;
#endif /* TOPPERS_SUPPORT_OVRHDR */

	while (true) {
#ifdef TOPPERS_SUPPORT_OVRHDR
		SVC_PERROR(ref_ovr(TSK_SELF, &pk_rovr));
		if ((pk_rovr.ovrstat & TOVR_STA) != 0) {
			syslog(LOG_NOTICE, "task%d is running (%03d).   %s  [%ld]",
								tskno, ++n, graph[tskno-1], pk_rovr.leftotm);
		}
		else {
			syslog(LOG_NOTICE, "task%d is running (%03d).   %s",
										tskno, ++n, graph[tskno-1]);
		}
#else /* TOPPERS_SUPPORT_OVRHDR */
		syslog(LOG_NOTICE, "task%d is running (%03d).   %s",
										tskno, ++n, graph[tskno-1]);
#endif /* TOPPERS_SUPPORT_OVRHDR */
		for (i = 0; i < task_loop; i++);
		c = message[tskno-1];
		message[tskno-1] = 0;
		switch (c) {
		case 'e':
			syslog(LOG_INFO, "#%d#ext_tsk()", tskno);
			SVC_PERROR(ext_tsk());
			assert(0);
		case 's':
			syslog(LOG_INFO, "#%d#slp_tsk()", tskno);
			SVC_PERROR(slp_tsk());
			break;
		case 'S':
			syslog(LOG_INFO, "#%d#tslp_tsk(10000000)", tskno);
			SVC_PERROR(tslp_tsk(10000000));
			break;
		case 'd':
			syslog(LOG_INFO, "#%d#dly_tsk(10000000)", tskno);
			SVC_PERROR(dly_tsk(10000000));
			break;
		case 'y':
			syslog(LOG_INFO, "#%d#dis_ter()", tskno);
			SVC_PERROR(dis_ter());
			break;
		case 'Y':
			syslog(LOG_INFO, "#%d#ena_ter()", tskno);
			SVC_PERROR(ena_ter());
			break;
#ifdef CPUEXC1
		case 'z':
			syslog(LOG_NOTICE, "#%d#raise CPU exception", tskno);
			RAISE_CPU_EXCEPTION;
			break;
		case 'Z':
			SVC_PERROR(loc_cpu());
			syslog(LOG_NOTICE, "#%d#raise CPU exception", tskno);
			RAISE_CPU_EXCEPTION;
			SVC_PERROR(unl_cpu());
			break;
#endif /* CPUEXC1 */
		default:
			break;
		}
	}
}
Exemplo n.º 28
0
void
task1(intptr_t exinf)
{
	ER		ercd;
	T_RTSK	rtsk;
	T_RMTX	rmtx;

	set_bit_func(bit_mutex);

	check_point(1);
	ercd = ref_mtx(MTX1, &rmtx);
	check_ercd(ercd, E_OK);
	check_assert(rmtx.htskid == TSK_NONE);
	check_assert(rmtx.wtskid == TSK_NONE);

	ercd = loc_mtx(MTX1);
	check_ercd(ercd, E_OK);

	check_point(2);
	ercd = ref_tsk(TASK1, &rtsk);
	check_ercd(ercd, E_OK);
	check_assert(rtsk.tskpri == HIGH_PRIORITY);
	check_assert(rtsk.tskbpri == LOW_PRIORITY);

	ercd = loc_mtx(MTX1);
	check_ercd(ercd, E_ILUSE);

	check_point(3);
	ercd = ref_mtx(MTX1, &rmtx);
	check_ercd(ercd, E_OK);
	check_assert(rmtx.htskid == TASK1);
	check_assert(rmtx.wtskid == TSK_NONE);

	ercd = act_tsk(TASK2);
	check_ercd(ercd, E_OK);

	check_point(4);
	ercd = tslp_tsk(10);
	check_ercd(ercd, E_TMOUT);

	check_point(6);
	ercd = ref_mtx(MTX1, &rmtx);
	check_ercd(ercd, E_OK);
	check_assert(rmtx.htskid == TASK1);
	check_assert(rmtx.wtskid == TASK2);

	ercd = act_tsk(TASK3);
	check_ercd(ercd, E_OK);

	check_point(7);
	ercd = tslp_tsk(10);
	check_ercd(ercd, E_TMOUT);

	check_point(10);
	ercd = ref_mtx(MTX1, &rmtx);
	check_ercd(ercd, E_OK);
	check_assert(rmtx.htskid == TASK1);
	check_assert(rmtx.wtskid == TASK3);

	ercd = dis_dsp();
	check_ercd(ercd, E_OK);

	ercd = unl_mtx(MTX1);
	check_ercd(ercd, E_OK);

	check_point(11);
	ercd = ref_tsk(TASK1, &rtsk);
	check_ercd(ercd, E_OK);
	check_assert(rtsk.tskpri == LOW_PRIORITY);
	check_assert(rtsk.tskbpri == LOW_PRIORITY);

	ercd = ref_mtx(MTX1, &rmtx);
	check_ercd(ercd, E_OK);
	check_assert(rmtx.htskid == TASK3);
	check_assert(rmtx.wtskid == TASK2);

	ercd = ena_dsp();
	check_ercd(ercd, E_OK);

	check_point(16);
	ercd = ref_mtx(MTX1, &rmtx);
	check_ercd(ercd, E_OK);
	check_assert(rmtx.htskid == TASK2);
	check_assert(rmtx.wtskid == TSK_NONE);

	ercd = tloc_mtx(MTX1, 10);
	check_ercd(ercd, E_TMOUT);

	check_point(17);
	ercd = wup_tsk(TASK2);
	check_ercd(ercd, E_OK);

	check_finish(20);

	check_point(0);
}
Exemplo n.º 29
0
/* メインタスク */
void main_task(intptr_t unused)
{
    signed char forward;      /* 前後進命令 */
    signed char turn;         /* 旋回命令 */
    signed char pwm_L, pwm_R; /* 左右モータPWM出力 */

    /* LCD画面表示 */
    ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
    ev3_lcd_draw_string("ミヤウチ", 0, CALIB_FONT_HEIGHT*1);

    /* センサー入力ポートの設定 */
    ev3_sensor_config(sonar_sensor, ULTRASONIC_SENSOR);
    ev3_sensor_config(color_sensor, COLOR_SENSOR);
    ev3_color_sensor_get_reflect(color_sensor); /* 反射率モード */
    ev3_sensor_config(touch_sensor, TOUCH_SENSOR);
    ev3_sensor_config(gyro_sensor, GYRO_SENSOR);
    /* モーター出力ポートの設定 */
    ev3_motor_config(left_motor, LARGE_MOTOR);
    ev3_motor_config(right_motor, LARGE_MOTOR);
    ev3_motor_config(tail_motor, LARGE_MOTOR);
    ev3_motor_reset_counts(tail_motor);
    
     //キャリブレイト 白取得
    printf("Press the touch sensor to measure light intensity of WHITE.\n");
    while(!ev3_touch_sensor_is_pressed(touch_sensor));
    while(ev3_touch_sensor_is_pressed(touch_sensor));
    int white = ev3_color_sensor_get_reflect(color_sensor);
    printf("WHITE light intensity: %d.\n", white);

    //キャリブレイト 黒取得
    printf("Press the touch sensor to measure light intensity of BLACK.\n");
    while(!ev3_touch_sensor_is_pressed(touch_sensor));
    while(ev3_touch_sensor_is_pressed(touch_sensor));
    int black = ev3_color_sensor_get_reflect(color_sensor);
    printf("BLACK light intensity: %d.\n", black);

    //PID制御
    float lasterror = 0, integral = 0;
    float midpoint = (white - black) / 2 + black;

    /* Open Bluetooth file */
    bt = ev3_serial_open_file(EV3_SERIAL_BT);
    assert(bt != NULL);

    /* Bluetooth通信タスクの起動 */
    act_tsk(BT_TASK);

    ev3_led_set_color(LED_ORANGE); /* 初期化完了通知 */

    /* スタート待機 */
    while(1)
    {
    //尻尾さげる
        tail_control(TAIL_ANGLE_STAND_UP); 

        if (bt_cmd == 1)
        {
            //リモートスタート
            break; 
        }

        if (ev3_touch_sensor_is_pressed(touch_sensor) == 1)
        {
            //タッチセンサが押された
            break; 
        }

        tslp_tsk(1);
    }

    /* 走行モーターエンコーダーリセット */
    ev3_motor_reset_counts(left_motor);
    ev3_motor_reset_counts(right_motor);

    /* ジャイロセンサーリセット */
    ev3_gyro_sensor_reset(gyro_sensor);
    balancer.init(GYRO_OFFSET);                // <1>

	/* スタート(LED緑色) */
    ev3_led_set_color(LED_GREEN); 

    while(1)
	{
        int32_t motor_ang_l, motor_ang_r;
        int gyro, volt;

        if (ev3_button_is_pressed(BACK_BUTTON)) break;

        tail_control(TAIL_ANGLE_DRIVE); /* バランス走行用角度に制御 */

		forward = 30;
		
        float error = midpoint - ev3_color_sensor_get_reflect(color_sensor);
        integral = error + integral * 0.5;
        turn = 0.07 * error + 0.3 * integral + 1 * (error - lasterror);
           // float steer = 0.07 * error + 0.3 * integral + 1 * (error - lasterror);
           // ev3_motor_steer(left_motor, right_motor, 10, steer);
        lasterror = error;
        tslp_tsk(1);

        /* 倒立振子制御API に渡すパラメータを取得する */
        motor_ang_l = ev3_motor_get_counts(left_motor);
        motor_ang_r = ev3_motor_get_counts(right_motor);
        gyro = ev3_gyro_sensor_get_rate(gyro_sensor);
        volt = ev3_battery_voltage_mV();

        /* 倒立振子制御APIを呼び出し、倒立走行するための */
        /* 左右モータ出力値を得る */
        balancer.setCommand(forward, turn);   // <1>
        balancer.update(gyro, motor_ang_r, motor_ang_l, volt); // <2>
        pwm_L = balancer.getPwmRight();       // <3>
        pwm_R = balancer.getPwmLeft();        // <3>

        /* EV3ではモーター停止時のブレーキ設定が事前にできないため */
        /* 出力0時に、その都度設定する */
        if (pwm_L == 0)
        {
             ev3_motor_stop(left_motor, true);
        }
        else
        {
            ev3_motor_set_power(left_motor, (int)pwm_L);
        }

        if (pwm_R == 0)
        {
             ev3_motor_stop(right_motor, true);
        }
        else
        {
         ev3_motor_set_power(right_motor, (int)pwm_R);
        }
	

        tslp_tsk(4); /* 4msec周期起動 */
	}
    ev3_motor_stop(left_motor, false);
    ev3_motor_stop(right_motor, false);

    ter_tsk(BT_TASK);
    fclose(bt);

    ext_tsk();
}
Exemplo n.º 30
0
void
task1(intptr_t exinf)
{
	ER_UINT	ercd;
	T_RTSK	rtsk;
	T_RMTX	rmtx;

	test_start(__FILE__);

	set_bit_func(bit_kernel);

	check_point(1);
	ercd = act_tsk(TASK2);
	check_ercd(ercd, E_OK);

	ercd = sta_alm(ALM1, TEST_TIME_CP);
	check_ercd(ercd, E_OK);

	ercd = slp_tsk();
	check_ercd(ercd, E_OK);

	check_point(4);
	ercd = loc_cpu();
	check_ercd(ercd, E_OK);

	ercd = ras_ter(TASK2);
	check_ercd(ercd, E_CTX);

	ercd = unl_cpu();
	check_ercd(ercd, E_OK);

	check_point(5);
	ercd = ras_ter(0U);
	check_ercd(ercd, E_ID);

	ercd = ras_ter(TNUM_TSKID + 1);
	check_ercd(ercd, E_ID);

	ercd = ras_ter(TASK1);
	check_ercd(ercd, E_ILUSE);

	ercd = ras_ter(TASK3);
	check_ercd(ercd, E_OBJ);

	check_point(6);
	ercd = ref_tsk(TASK2, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_RDY);

	check_point(7);
	ercd = ras_ter(TASK2);
	check_ercd(ercd, E_OK);

	check_point(8);
	ercd = ref_tsk(TASK2, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_DMT);

	check_point(9);
	ercd = act_tsk(TASK3);
	check_ercd(ercd, E_OK);

	check_point(12);
	ercd = ref_tsk(TASK3, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_WAI);

	ercd = ref_mtx(MTX1, &rmtx);
	check_ercd(ercd, E_OK);

	check_assert(rmtx.htskid == TASK3);

	check_assert(rmtx.wtskid == TSK_NONE);

	check_point(13);
	ercd = ras_ter(TASK3);
	check_ercd(ercd, E_OK);

	check_point(14);
	ercd = ref_tsk(TASK3, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_DMT);

	ercd = ref_mtx(MTX1, &rmtx);
	check_ercd(ercd, E_OK);

	check_assert(rmtx.htskid == TSK_NONE);

	check_assert(rmtx.wtskid == TSK_NONE);

	check_point(15);
	ercd = act_tsk(TASK3);
	check_ercd(ercd, E_OK);

	check_point(18);
	ercd = act_tsk(TASK4);
	check_ercd(ercd, E_OK);

	check_point(20);
	ercd = ref_tsk(TASK3, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_WAI);

	ercd = ref_mtx(MTX1, &rmtx);
	check_ercd(ercd, E_OK);

	check_assert(rmtx.htskid == TASK3);

	check_assert(rmtx.wtskid == TASK4);

	check_point(21);
	ercd = ras_ter(TASK3);
	check_ercd(ercd, E_OK);

	check_point(24);
	ercd = act_tsk(TASK2);
	check_ercd(ercd, E_OK);

	ercd = act_tsk(TASK2);
	check_ercd(ercd, E_OK);

	check_point(25);
	ercd = ref_tsk(TASK2, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_RDY);

	check_assert(rtsk.actcnt == 1U);

	check_point(26);
	ercd = ras_ter(TASK2);
	check_ercd(ercd, E_OK);

	check_point(27);
	ercd = ref_tsk(TASK2, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_RDY);

	check_assert(rtsk.actcnt == 0U);

	check_point(28);
	ercd = act_tsk(TASK3);
	check_ercd(ercd, E_OK);

	check_point(30);
	ercd = act_tsk(TASK3);
	check_ercd(ercd, E_OK);

	check_point(31);
	ercd = ref_tsk(TASK3, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_WAI);

	check_assert(rtsk.actcnt == 1U);

	check_point(32);
	ercd = ras_ter(TASK3);
	check_ercd(ercd, E_OK);

	check_point(34);
	ercd = act_tsk(TASK2);
	check_ercd(ercd, E_OK);

	ercd = slp_tsk();
	check_ercd(ercd, E_OK);

	check_point(36);
	ercd = ref_tsk(TASK2, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_RDY);

	check_assert(rtsk.raster == false);

	check_assert(rtsk.dister == true);

	check_point(37);
	ercd = ras_ter(TASK2);
	check_ercd(ercd, E_OK);

	check_point(38);
	ercd = ref_tsk(TASK2, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_RDY);

	check_assert(rtsk.raster == true);

	check_assert(rtsk.dister == true);

	check_point(39);
	ercd = ter_tsk(TASK2);
	check_ercd(ercd, E_OK);

	ercd = act_tsk(TASK2);
	check_ercd(ercd, E_OK);

	check_point(40);
	ercd = ref_tsk(TASK2, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_RDY);

	check_assert(rtsk.raster == false);

	check_assert(rtsk.dister == false);

	ercd = tslp_tsk(TEST_TIME_CP);
	check_ercd(ercd, E_TMOUT);

	check_point(42);
	ercd = ref_tsk(TASK2, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_WAI);

	check_assert(rtsk.raster == false);

	check_assert(rtsk.dister == true);

	check_point(43);
	ercd = ras_ter(TASK2);
	check_ercd(ercd, E_OK);

	check_point(44);
	ercd = ref_tsk(TASK2, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_RDY);

	check_assert(rtsk.raster == true);

	check_assert(rtsk.dister == true);

	ercd = slp_tsk();
	check_ercd(ercd, E_OK);

	check_point(46);
	ercd = act_tsk(TASK3);
	check_ercd(ercd, E_OK);

	check_point(48);
	ercd = ref_tsk(TASK3, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_WAI);

	check_assert(rtsk.raster == false);

	check_assert(rtsk.dister == true);

	check_point(49);
	ercd = ras_ter(TASK3);
	check_ercd(ercd, E_OK);

	check_point(52);
	ercd = ref_tsk(TASK3, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_DMT);

	check_point(53);
	ercd = act_tsk(TASK3);
	check_ercd(ercd, E_OK);

	check_point(55);
	ercd = ref_tsk(TASK3, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_SUS);

	check_point(56);
	ercd = ras_ter(TASK3);
	check_ercd(ercd, E_OK);

	check_point(58);
	ercd = ref_tsk(TASK3, &rtsk);
	check_ercd(ercd, E_OK);

	check_assert(rtsk.tskstat == TTS_DMT);

	check_finish(59);
	check_point(0);
}