/*
* メインタスク
*/
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();
}
/*
* メインタスク
*/
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);
}
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);
}
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;
}
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));
}
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
}
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);
}
/*
* タッチセンサハンドラ
*/
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));
}
}
/*
* 並行実行されるタスク
*/
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;
}
}
}
/*
* 駆動ハンドラ
* 機能: 駆動タスクを起床する
*/
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));
}
}
/*
* 並行実行されるタスク
*/
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;
}
}
}
/*
* 並行実行されるタスク
*/
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;
}
}
}
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;
}
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);
}
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;
}
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;
}
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);
}
}
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);
}
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);
}
}
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);
}
/*
* 並行実行されるタスク
*/
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);
}
}
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);
}
}
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));
}
}
void RTOS::waiTask(uint16 rv_timeout_cnt){
tslp_tsk(rv_timeout_cnt);
}
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
}
/*
* 並行実行されるタスク
*/
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;
}
}
}
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);
}
/* メインタスク */
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();
}
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);
}