/** * Checks position of "Sleep" (standby) switch at the back of the device and * take action if switch position changed. */ void Controller::check_sleep_switch() { #ifndef NEVERSLEEP bool sstate = switch_state(); if(sstate != m_last_switch_state) { m_last_switch_state = sstate; if(sstate == false) { if(m_alarm_log && (!m_sleeping)) { m_sleeping=true; display_powerdown(); } else { m_sleeping=true; power_standby(); } } if(sstate == true) { m_powerup = true; } } #endif if(m_powerup == true) { buzzer_nonblocking_buzz(0.5); display_powerup(); m_gui->set_sleeping(false); m_gui->redraw(); m_sleeping=false; m_powerup =false; const char *devicetag = flashstorage_keyval_get("DEVICETAG"); char revtext[10]; sprintf(revtext,"VERSION: %s ",OS100VERSION); display_splashscreen(devicetag,revtext); delay_us(3000000); display_clear(0); } if(m_sleeping) { // go back to sleep. if((!rtc_alarmed()) && (!m_alarm_log)) { power_standby(); } return; } }
void submenu_callback(void) { lcd_screen_activate(0); submenuState++; if(submenuState == 6) submenuState = 0; display_symbol(0, LCD_SEG_L2_COL0, SEG_OFF); if (submenuState == 0) { display_clear(0, 2); display_chars(0, LCD_SEG_L2_5_0, NULL, BLINK_OFF); } else { display_chars(0, LCD_SEG_L2_5_0, pre_str[submenuState-1], SEG_SET); display_chars(0, LCD_SEG_L2_5_0, NULL, BLINK_ON); } update(SYS_MSG_FAKE); }
void display_gui_alert (const char* title, const char* message, const char* button1, const char* button2, void (*callback) (bool)) { current_screen = display_gui_screen_alert; gui_alert_callback = callback; display_gui_a_function = button1; display_gui_b_function = button2; display_gui_left_available = false; display_gui_up_available = false; display_gui_right_available = false; display_gui_down_available = false; display_gui_button_bar_visible = true; display_clear(); display_draw_rect (0, DISPLAY_FONT0_HEIGHT, DISPLAY_WIDTH - 1, DISPLAY_HEIGHT - BUTTON_INFO_BAR_HEIGHT - 2, false); display_draw_string (0, 0, 0, title); display_draw_string (3, DISPLAY_FONT0_HEIGHT + 3, 0, message); }
void accelerometer_pixel_bars(void){ short accels[3]; int xpix,ypix; char x_acc_dir,y_acc_dir; unsigned char x_dir[20],y_dir[20]; acc_read_register(OUT_X_L_A,(unsigned char *) accels, 6); xpix = accel_to_pixels(accels[0]); ypix = accel_to_pixels(accels[1]); x_acc_dir = accel_pixel_direction(xpix); y_acc_dir = accel_pixel_direction(ypix); sprintf(x_dir,"X-Acc = %c",x_acc_dir); sprintf(y_dir,"Y-Acc = %c",y_acc_dir); display_clear(); write_OLED_message(x_dir,0,0); write_OLED_message(y_dir,10,0); accel_pixels(xpix,'x'); accel_pixels(ypix,'y'); display_draw(); }
void display_map(int width, int height, U8 matrix[width][height]) { int x=__min_x,y=__min_y; coord_to_table_index(&x,&y); display_clear(0); for( int i = 0; i < width; ++i ) { for( int j = 0; j < height; ++j ) { display_goto_xy(i,j); U8 result = 0x00; result |= ( matrix[x][y] & W_WALL_MASK ); result |= ( ( matrix[x][y] & E_WALL_MASK ) >> 1 ); result |= ( ( matrix[x][y] & S_WALL_MASK ) >> 2 ); result |= ( ( matrix[x][y] & N_WALL_MASK ) >> 3 ); display_hex( result, 1 ); y=(y+1) % height; } x=(x+1) % width; } display_update(); }
void handle_clock(void) { if (minute == 59) { minute = 0; if (hour == 23) hour = 0; else hour++; total_hours++; } else { minute++; } display_clear(1); display_putint((hour * 100) + minute, 0, 1); display_putint(total_hours, 1, 0); display_init(1); display_update(1); }
int main(){ camera_open(LOW_RES); camera_load_config("color.config"); while(!(a_button_clicked())){ display_clear(); camera_update(); int objNum = get_object_count(GREEN); if(objNum == 0) printf("NO SUCH OBJECT!!!!!!!!"); else if(objNum > 0){ point2 objCen = get_object_center(GREEN, 0); int objArea = get_object_area(GREEN, 0); printf("X:\t%d\nY:\t%d\n", objCen.x, objCen.y); printf("Area:\t%d\n", objArea); } msleep(100); } return 0; }
//* ************************************************************************************************ /// @fn main(void) /// @brief Application's entry point /// @return int Exit status //* ************************************************************************************************ int main(void) { // Init MCU init_application(); #ifdef CONFIG_TEST /// @todo Remove this ? // Branch to welcome screen test_mode(); #else // Clear whole scren display_clear(0, 0); #endif // Init modules mod_init(); // Main loop while (1) { // Go to LPM3, wait for interrupts _BIS_SR(LPM3_bits + GIE); __no_operation(); // Service watchdog on wakeup #ifdef USE_WATCHDOG // Service watchdog (reset counter) WDTCTL = (WDTCTL & 0xff) | WDTPW | WDTCNTCL; #endif // Check if any driver has events pending check_events(); // Check for button presses, drive the menu check_buttons(); } }
//***************************************************************************** // 関数名 : calibration // 引数 : *black (黒、最小値),*white(白、最大値) // 返り値 : 無し // 概要 : 光センサの手動キャリブレーション // 黒白の順でタッチする。 //***************************************************************************** void calibration(int *black,int *white,int angle) { while(1) { tail_control(angle); if (ecrobot_get_touch_sensor(NXT_PORT_S4) == 1) { ecrobot_sound_tone(440, 170, 100); *black = ecrobot_get_light_sensor(NXT_PORT_S3); display_clear(0); /* 画面表示 */ display_goto_xy(0, 1); display_string("BLACK:"); display_int(*black, 4); break; }//if systick_wait_ms(100); /* 10msecウェイト */ }//while display_update(); while(ecrobot_get_touch_sensor(NXT_PORT_S4)); ecrobot_sound_tone(880, 170, 100); while(1) { tail_control(angle); if (ecrobot_get_touch_sensor(NXT_PORT_S4) == 1) { ecrobot_sound_tone(880, 170, 100); *white = ecrobot_get_light_sensor(NXT_PORT_S3); //display_clear(0); /* 画面表示 */ display_goto_xy(0, 2); display_string("WHITE:"); display_int(*white, 4); break; }//if systick_wait_ms(100); /* 10msecウェイト */ }//while //display_clear(0); /* 画面表示 */ display_goto_xy(0,4); display_string("TH:"); display_int(TH(*black,*white), 3); display_update(); while(ecrobot_get_touch_sensor(NXT_PORT_S4)); ecrobot_sound_tone(440, 170, 100); }
void component_init() { power_screen(true); LED_Light(led_red, true); LED_Light(led_green, true); LED_Light(led_blue, true); screen_step(0); RTC_Clock clock = { 0, 0, 30, 23, 0, 8, 8, 12 }; SetTime(&clock); screen_step(1); screen_step(2); mobile_state(); screen_step(3); screen_step(4); prepare_mobile(false); screen_step(5); play_media(rand() % 9 + 1); screen_step(6); { char szSize[piece_size]; sprintf(szSize, "mount tf card %s", (pf_open("temp") == FR_OK) ? "correcttly" : "fault"); post_trace(szSize); } screen_step(7); screen_step(8); report_content("初始化过程完成,现在开始运行测试程序"); screen_step(9); screen_step(10); LED_Light(led_red, false); LED_Light(led_green, false); LED_Light(led_blue, false); display_clear(); power_screen(false); }
// prepare: void prepare(){ camera_open(LOW_RES); camera_load_config(CONFIG); camera_update(); camera_update(); camera_update(); // prepare camera enable_servos(); set_servo_position(BSV, BSV_LEVEL); set_servo_position(ASV, ASV_BACK); set_servo_position(RSV, RSV_DOWN); set_servo_position(SSV, SSV_BACK); // prepare servos display_clear(); printf("prepare succeeds"); while(!(a_button_clicked())); // show controller that coke is ready }
void update(enum sys_message msg) { read_altitude(); display_altitude(sAlt.altitude, 0); display_climb(sAlt.climb, ALT_SCREEN_CLIMB); display_altitude(sAlt.minAltitude, ALT_SCREEN_MIN); display_altitude(sAlt.maxAltitude, ALT_SCREEN_MAX); display_altitude(sAlt.accuClimbDown, ALT_SCREEN_ACC_N); display_altitude(sAlt.accuClimbUp, ALT_SCREEN_ACC_P); if((accelerometer == 1) &&(submenuState == 0)){ display_clear(0 ,2); display_chars(0, LCD_SEG_L2_4_0, "ACCEL", SEG_SET); } time_callback(SYS_MSG_RTC_HOUR | SYS_MSG_RTC_MINUTE); }
void message_reply_task(void *p) { post_trace("start"); power_screen(true); display_clear(); show_picture(15, 30, IMAGE_LOGO); prepare_mobile(false); while(1) { if(KEY_Pressed(KEY_State(), key_service)) { while(KEY_Pressed(KEY_State(), key_service)); send_same_message(); }else if(KEY_Pressed(KEY_State(), key_broadcast)) { while(KEY_Pressed(KEY_State(), key_broadcast)); send_differ_message(); } else if(KEY_PowerOff()) { POWER_Off(); return; } } }
int main() { BM_setup(); display_clear(); acc_write_register(0x00,0x04,(unsigned char *) buff,2); sprintf(message,"%d%d",buff[1],buff[2]); oledprint(1,10,message); display_draw(); // while (1) { // // display_clear(); // // acc_write_register(RDCVA, buff); // // sprintf(buff,"Message: ",data); // // oledprint(1,10,buff); // display_draw(); // } }
/* ******************************************************************** * * public: display the current menu * @param * @return * ******************************************************************** */ void LCDMenuLib::display() /* ******************************************************************** */ { //declaration LCDMenu * tmp; uint8_t i = scroll; uint8_t maxi = (rows + scroll); char buffer[_LCDML_DISP_cfg_max_string_length]; child_cnt = countChilds(); //check children if ((tmp = curMenu->getChild(i))) { if (!bitRead(control, _LCDML_control_search_display)) { //clear lcd display_clear(); //show menu structure do { if (bitRead(group_en, tmp->disp)) { #if defined ( ESP8266 ) || defined ( ESP32 ) strcpy_P(content[i-scroll], (char*)(flash_table[tmp->name])); #else strcpy_P(content[i-scroll], (char*)pgm_read_word(&(flash_table[tmp->name]))); #endif content_id[i-scroll] = tmp->name; i++; } } while ((tmp = tmp->getSibling(1)) != NULL && i<maxi); bitSet(control, _LCDML_control_disp_update); } } else { // no children goBack(); // function can run ... } setCursor(); }
void game_draw(void) { int i; // reset buffer at startup display_clear(); // Draw players players_draw(); // Draw ball display_drawCircle(ball->Location.x, ball->Location.y, ball->Radius); // Draw goals for(i = 20; i < LCD_HEIGHT - 20; i++) { display_drawPixel(5, i, 1); display_drawPixel(LCD_WIDTH - 6, i, 1); } display_flush(); }
static void altitude_deactivate(void) { sys_messagebus_unregister(&update); sys_messagebus_unregister(&time_callback); // Clean up function-specific segments before leaving function display_symbol(0, LCD_SYMB_ARROW_UP, SEG_OFF); display_symbol(0, LCD_SYMB_ARROW_DOWN, SEG_OFF); if(useMetric){ display_symbol(0, LCD_UNIT_L1_M, SEG_OFF); }else{ display_symbol(0, LCD_UNIT_L1_FT, SEG_OFF); } //update_pressure_table((s16) sAlt.raw_altitude, sAlt.pressure, sAlt.temperature); lcd_screens_destroy(); display_clear(0, 0); }
void menu (display *tela) { char code; display_clear(tela); display_puts(tela, tela->width/2 - 9, tela->height/2, "Cuidado onde pisa."); display_puts(tela, tela->width/2 - 9, tela->height/2 - 1, "Menu:"); display_puts(tela, tela->width/2 - 5, tela->height/2 - 2, "1. Start"); display_puts(tela, tela->width/2 - 5, tela->height/2 - 3, "2. Exit"); display_show(tela); code = getch(); while(code != '1') { if(code == '2') exit(0); printf("\nDigite um código valido: "); code = getch(); } // Quando o jogo começa. g_start = clock() / CLOCKS_PER_SEC; }
// prepare: void prepare(){ camera_open(LOW_RES); camera_load_config(CONFIG); camera_update(); camera_update(); camera_update(); // prepare camera enable_servos(); set_servo_position(BSV, BSV_LEVEL); set_servo_position(ASV, ASV_BACK); set_servo_position(RSV, RSV_DOWN); set_servo_position(SSV, SSV_BACK); // prepare servos display_clear(); printf("prepare succeeds"); while(digital(TSS) == 0); msleep(1500); //wait_for_light(SSS); //shut_down_in(TIME_LIMIT); // show controller that coke is ready }
static void display_string_with_offset(U8 *data, int offset) { char buffer[MAX_NUM_OF_CHAR+1]; int offset_local = offset; int iterate = 1; size_t len; size_t n_bytes; /* iterate in case the string is longer than MAX_NUM_OF_CHAR */ while(iterate) { len = strlen((char *)&data[offset_local]); n_bytes = MAX_NUM_OF_CHAR; if (len < MAX_NUM_OF_CHAR) { n_bytes = len; iterate = 0; } memcpy(buffer, (char *)&data[offset_local], n_bytes); buffer[n_bytes] = 0; offset_local += n_bytes; if (pos_x == 0 && pos_y == 0) { display_clear(0); } /* update and set the cursor position */ display_goto_xy(pos_x, pos_y); display_string(buffer); display_update(); pos_y = pos_y+1; if (pos_y >= MAX_NUM_OF_LINE) { pos_x = 0; pos_y = 0; } } }
LOWERCODESIZE static void prepareSynth(void) { display_clear(); led_set(plTune,1,0); #ifdef DEBUG sh_setCV(pcMVol,20000,0); #else sh_setCV(pcMVol,0,0); #endif // Update CV's and give final volume VCA CV filter time to close sh_update(); MDELAY(150); sh_setGate(pgASaw,0); sh_setGate(pgATri,0); sh_setGate(pgBSaw,0); sh_setGate(pgBTri,0); sh_setGate(pgPModFA,0); sh_setGate(pgPModFil,0); sh_setGate(pgSync,0); sh_setCV(pcResonance,0,0); sh_setCV(pcAPW,0,0); sh_setCV(pcBPW,0,0); sh_setCV(pcPModOscB,0,0); sh_setCV(pcExtFil,0,0); // init 8253 // ch 0, mode 0, access 2 bytes, binary count i8253Write(0x3,0b00110000); // ch 1, mode 0, access 2 bytes, binary count i8253Write(0x3,0b01110000); // ch 2, mode 1, access 2 bytes, binary count i8253Write(0x3,0b10110010); }
void tf_show(void *p) { int i=0; bool bRet = true; initialize(); power_screen(true); while(true){ for ( i=0; i<55; i++){ bRet = show_picture(9, 9, SHOW_PIC_PATH[i]); if(!bRet){ break; } display_clear(); } if(!bRet){ break; } } while(true){ sprintf(watcher_data->temp_path, "RES=%d", Res); post_trace(watcher_data->temp_path); } }
void draw() { static uint8_t p1 = 0, p2 = 0, p3 = 0, p4 = 0; static uint32_t rand = 12345; uint8_t x, y, t1, t2, t3, t4, z, z0; display_clear(); t1 = p1; t2 = p2; for (y = 0; y < 64; y++) { t3 = p3; t4 = p4; z0 = sin_table[t1] + sin_table[t2]; for (x = 0; x < 128; x++) { z = z0 + sin_table[t3] + sin_table[t4]; z = z >> 2; if (z > pattern8x8[x & 7][y & 7]) display_setPixel(x, y, WHITE); t3 += 1; t4 += 2; } t1 += 1; t2 += 2; } display_update(); p1 += rand % 4; p2 -= 2; p3 += 3; p4 -= 1; rand = rand*109 + 89; }
void select_loop(void) { fd_set readfd; fd_set writefd; int anyset = 0; int maxfd = 0; int dnsfd, netfd; #ifdef ENABLE_IPV6 int dnsfd6; #endif int NumPing = 0; int paused = 0; struct timeval lasttime, thistime, selecttime; struct timeval startgrace; int dt; int rv; int graceperiod = 0; memset(&startgrace, 0, sizeof(startgrace)); gettimeofday(&lasttime, NULL); while(1) { dt = calc_deltatime (WaitTime); intervaltime.tv_sec = dt / 1000000; intervaltime.tv_usec = dt % 1000000; FD_ZERO(&readfd); FD_ZERO(&writefd); maxfd = 0; if(Interactive) { FD_SET(0, &readfd); maxfd = 1; } #ifdef ENABLE_IPV6 if (dns) { dnsfd6 = dns_waitfd6(); if (dnsfd6 >= 0) { FD_SET(dnsfd6, &readfd); if(dnsfd6 >= maxfd) maxfd = dnsfd6 + 1; } else { dnsfd6 = 0; } } else dnsfd6 = 0; #endif if (dns) { dnsfd = dns_waitfd(); FD_SET(dnsfd, &readfd); if(dnsfd >= maxfd) maxfd = dnsfd + 1; } else dnsfd = 0; netfd = net_waitfd(); FD_SET(netfd, &readfd); if(netfd >= maxfd) maxfd = netfd + 1; if (mtrtype == IPPROTO_TCP) net_add_fds(&writefd, &maxfd); do { if(anyset || paused) { /* Set timeout to 0.1s. * While this is almost instantaneous for human operators, * it's slow enough for computers to go do something else; * this prevents mtr from hogging 100% CPU time on one core. */ selecttime.tv_sec = 0; selecttime.tv_usec = paused?100000:0; rv = select(maxfd, (void *)&readfd, &writefd, NULL, &selecttime); } else { if(Interactive) display_redraw(); gettimeofday(&thistime, NULL); if(thistime.tv_sec > lasttime.tv_sec + intervaltime.tv_sec || (thistime.tv_sec == lasttime.tv_sec + intervaltime.tv_sec && thistime.tv_usec >= lasttime.tv_usec + intervaltime.tv_usec)) { lasttime = thistime; if (!graceperiod) { if (NumPing >= MaxPing && (!Interactive || ForceMaxPing)) { graceperiod = 1; startgrace = thistime; } /* do not send out batch when we've already initiated grace period */ if (!graceperiod && net_send_batch()) NumPing++; } } if (graceperiod) { dt = (thistime.tv_usec - startgrace.tv_usec) + 1000000 * (thistime.tv_sec - startgrace.tv_sec); if (dt > GRACETIME) return; } selecttime.tv_usec = (thistime.tv_usec - lasttime.tv_usec); selecttime.tv_sec = (thistime.tv_sec - lasttime.tv_sec); if (selecttime.tv_usec < 0) { --selecttime.tv_sec; selecttime.tv_usec += 1000000; } selecttime.tv_usec = intervaltime.tv_usec - selecttime.tv_usec; selecttime.tv_sec = intervaltime.tv_sec - selecttime.tv_sec; if (selecttime.tv_usec < 0) { --selecttime.tv_sec; selecttime.tv_usec += 1000000; } if (dns) { if ((selecttime.tv_sec > (time_t)dnsinterval) || ((selecttime.tv_sec == (time_t)dnsinterval) && (selecttime.tv_usec > ((time_t)(dnsinterval * 1000000) % 1000000)))) { selecttime.tv_sec = (time_t)dnsinterval; selecttime.tv_usec = (time_t)(dnsinterval * 1000000) % 1000000; } } rv = select(maxfd, (void *)&readfd, NULL, NULL, &selecttime); } } while ((rv < 0) && (errno == EINTR)); if (rv < 0) { perror ("Select failed"); exit (1); } anyset = 0; /* Have we got new packets back? */ if(FD_ISSET(netfd, &readfd)) { net_process_return(); anyset = 1; } if (dns) { /* Handle any pending resolver events */ dnsinterval = WaitTime; dns_events(&dnsinterval); } /* Have we finished a nameservice lookup? */ #ifdef ENABLE_IPV6 if(dns && dnsfd6 && FD_ISSET(dnsfd6, &readfd)) { dns_ack6(); anyset = 1; } #endif if(dns && dnsfd && FD_ISSET(dnsfd, &readfd)) { dns_ack(); anyset = 1; } /* Has a key been pressed? */ if(FD_ISSET(0, &readfd)) { switch (display_keyaction()) { case ActionQuit: return; break; case ActionReset: net_reset(); break; case ActionDisplay: display_mode = (display_mode+1) % 3; break; case ActionClear: display_clear(); break; case ActionPause: paused=1; break; case ActionResume: paused=0; break; case ActionMPLS: enablempls = !enablempls; display_clear(); break; case ActionDNS: if (dns) { use_dns = !use_dns; display_clear(); } break; #ifdef IPINFO case ActionII: if (ipinfo_no >= 0) { ipinfo_no++; if (ipinfo_no > ipinfo_max) ipinfo_no = 0; } break; case ActionAS: if (ipinfo_no >= 0) ipinfo_no = ipinfo_no?0:ipinfo_max; break; #endif case ActionScrollDown: display_offset += 5; break; case ActionScrollUp: display_offset -= 5; if (display_offset < 0) { display_offset = 0; } break; } anyset = 1; } /* Check for activity on open sockets */ if (mtrtype == IPPROTO_TCP) net_process_fds(&writefd); } return; }
/** * The main render loop of the GUI */ void GUI::render() { if(m_sleeping) { // process_keys(); return; } if(m_displaying_dialog) { if(m_dialog_buzz) buzzer_nonblocking_buzz(1); return; } if(m_displaying_dialog_complete) { m_displaying_dialog_complete=false; m_pause_display_updates = false; display_clear(0); clear_pending_keys(); redraw(); } if(m_repeating) { // This would be better incremented in a timer, but I don't want to use another timer. if(m_repeat_time == m_repeat_delay) { // verify button still pressed if(cap_ispressed(m_repeat_key) == false) { m_repeating=false; m_repeat_time=0; } else { if(m_repeat_key == KEY_DOWN) { receive_key(KEY_DOWN,KEY_PRESSED); } if(m_repeat_key == KEY_UP ) { receive_key(KEY_UP ,KEY_PRESSED); } m_repeat_time = 0; m_repeated = true; } } m_repeat_time++; } // following two items really need to be atomic... int32_t cscreen = current_screen; if(clear_next_render) { clear_next_render=false; clear_screen(clear_screen_screen,clear_screen_selected); first_render=true; } bool do_redraw = false; if(m_redraw) do_redraw = true; m_redraw = false; render_lock(m_screen_lock); for(int32_t n=0;n<screens_layout[cscreen].item_count;n++) { if(first_render) { if(screens_layout[current_screen].items[n].type == ITEM_TYPE_ACTION) { receive_gui_events.receive_gui_event(screens_layout[cscreen].items[n].text, screens_layout[cscreen].items[n].text); } } //bool selected = false; bool select_render = false; if(n == selected_item ) select_render = true; if(n == last_selected_item) select_render = true; if(first_render || select_render || do_redraw) { //bool do_render = true; // don't render labels, just because they are near other things... //if(!first_render && select_render && (screens_layout[cscreen].items[n].type == ITEM_TYPE_LABEL)) { // do_render = false; //} //if(do_render) bool selected = false; if(selected_item == n) selected=true; render_item(screens_layout[cscreen].items[n],selected); } } //last_selected_item = selected_item; first_render=false; process_keys(); }
void findBall(){ display_clear(); int temptLMSpeed = LM_MINS; // changeable minimum speed for left motor int temptRMSpeed = RM_MINS; // changeable minimum speed for right morot int lastLMSpeed, lastRMSpeed; // motor speed int lastLMPos, lastRMPos; // motor positions int errorX = 0, errorY = 0; // difference of current ball coordinate and target coordinate int objArea; // area of the largest object in sight point2 objCen; // object center coordinate camera_update(); int objNum = get_object_count(GREEN); while(objNum == 0){ camera_update(); objNum = get_object_count(GREEN); } // find a ball in sight objCen = get_object_center(GREEN, 0); errorX = OFFSET_X - objCen.x; errorY = OFFSET_Y - objCen.y; while(!(a_button_clicked())){ camera_update(); objArea = get_object_area(GREEN, 0); while(objArea < 200){ ao(); camera_update(); objArea = get_object_area(GREEN, 0); } // make sure really a ball is in sight objCen = get_object_center(GREEN, 0); errorX = OFFSET_X - objCen.x; errorY = OFFSET_Y - objCen.y; // find differences between coordinates if(errorX > -3 && errorX < 2 && errorY > -3 && errorY < 2) break; int turnLM = -1 * errorX * P_X + errorY * P_Y; int turnRM = errorX * P_X + errorY * P_Y; // turn for LM, RM if(turnLM == lastLMSpeed && lastLMPos == get_motor_position_counter(LM)) temptLMSpeed = temptLMSpeed + 1; if(turnLM == lastLMSpeed && lastLMPos != get_motor_position_counter(LM)) temptLMSpeed = temptLMSpeed - 1; if(turnLM == -1 * lastLMSpeed && lastLMPos != get_motor_position_counter(LM)) temptLMSpeed = temptLMSpeed - 2; if(turnRM == lastRMSpeed && lastRMPos == get_motor_position_counter(RM)) temptRMSpeed = temptRMSpeed + 1; if(turnRM == lastRMSpeed && lastRMPos != get_motor_position_counter(RM)) temptRMSpeed = temptRMSpeed - 1; if(turnRM == -1 * lastRMSpeed && lastRMPos != get_motor_position_counter(RM)) temptRMSpeed = temptRMSpeed - 2; lastLMSpeed = turnLM; lastLMPos = get_motor_position_counter(LM); lastRMSpeed = turnRM; lastRMPos = get_motor_position_counter(RM); // check if speed is too high or low, thus make modification to minimum speed if(turnLM > -1 * temptLMSpeed && turnLM < 0) turnLM = -1 * temptLMSpeed; if(turnLM > 0 && turnLM < temptLMSpeed) turnLM = temptLMSpeed; if(turnRM > -1 * temptRMSpeed && turnRM < 0) turnRM = -1 * temptRMSpeed; if(turnRM > 0 && turnRM < temptRMSpeed) turnRM = temptRMSpeed; // set minimum speed: avoid unmoving motor(LM, turnLM); motor(RM, turnRM); // move } ao(); catchBall(GREEN); // stop and catch }
int main() { // Startup code to run as fast as possible and get pins back from bad defaults __builtin_disable_interrupts(); // set the CP0 CONFIG register to indicate that // kseg0 is cacheable (0x3) or uncacheable (0x2) // see Chapter 2 "CPU for Devices with M4K Core" // of the PIC32 reference manual __builtin_mtc0(_CP0_CONFIG, _CP0_CONFIG_SELECT, 0xa4210583); // no cache on this chip! // 0 data RAM access wait states BMXCONbits.BMXWSDRM = 0x0; // enable multi vector interrupts INTCONbits.MVEC = 0x1; // disable JTAG to be able to use TDI, TDO, TCK, TMS as digital DDPCONbits.JTAGEN = 0; __builtin_enable_interrupts(); // set up USER pin (RB13) as a digital input ANSELBbits.ANSB13 = 0; TRISBbits.TRISB13 = 1; // set up LED1 pin (RB7) as a digital output RPB7Rbits.RPB7R = 0b0001; TRISBbits.TRISB7 = 0; LATBbits.LATB7 = 1; // set up LED2 as OC1 using Timer2 at 1kHz RPB15Rbits.RPB15R = 0b0101; T2CONbits.TCKPS = 2; // Timer2 prescaler N=4 (1:4) PR2 = 9999; // period = (PR2+1) * N * 25 ns = 1 ms, 1 kHz TMR2 = 0; // initial TMR2 count is 0 OC1CONbits.OCM = 0b110; // PWM mode without fault pin; other OC1CON bits are defaults OC1RS = 5000; // duty cycle = OC1RS/(PR2+1) = 50% OC1R = 5000; // initialize before turning OC1 on; afterward it is read-only T2CONbits.ON = 1; // turn on Timer2 OC1CONbits.ON = 1; // turn on OC1 // set up A0 as AN0 ANSELAbits.ANSA0 = 1; AD1CON3bits.ADCS = 3; AD1CHSbits.CH0SA = 0; AD1CON1bits.ADON = 1; display_init(); display_clear(); char message[20]; int num = 1337; sprintf(message, "Hello World %d!",num); display_now(message, 28, 32); while (1) { _CP0_SET_COUNT(0); // set core timer to 0, remember it counts at half the CPU clock LATBINV = 0x0080; // invert a pin // wait for half a second, setting LED brightness to pot angle while waiting while (_CP0_GET_COUNT() < 10000000) { OC1RS = readADC() * PR2/1024; if (PORTBbits.RB13 == 1) { // nothing } else { LATBINV = 0x0080; } } } }
void display_init(void) { display_inited = true; display_clear(); }
void seesaw_ride( Command* cmd , ROBOT_STATUS* status ) { static s1 tail_seesaw = 0; static signed char left = 0; static signed char right = 0; f4 f4t_yp; f4 f4t_yd; static u2 u2_count = 0; static u2 u2_gyro_temp=0; static s4 s4t_distance = 0; static s4 s4t_motor_L1 = 0; static s4 s4t_motor_R1 = 0; static s4 s4t_motor_L2 = 0; static s4 s4t_motor_R2 = 0; static s4 s4t_motor_L3 = 0; static s4 s4t_motor_R3 = 0; static u1 u1s_trace = 0; static u1 u1s_touritu = 1; s4t_motor_L1 = nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R1 = nxt_motor_get_count(PORT_MOTOR_R); static u4 u4_recieve = 0; u1 receive_data[90] = {0}; /* user logic start */ u1s_2msflipcount_see = 1 - u1s_2msflipcount_see; display_clear(0); display_goto_xy(0, 0); display_string("stage"); display_unsigned(u1g_seesaw_ride_mode,3); display_goto_xy(0, 1); display_string("LEFT"); display_unsigned(left,3); display_goto_xy(0, 2); display_string("RIGHT"); display_unsigned(right,3); display_goto_xy(0, 3); display_string("count"); display_unsigned(f4g_forward,3); display_update(); if ( u1s_2msflipcount_see == 1 ) { switch(u1g_seesaw_ride_mode) /* シーソーシーケンス開始 */ { case 0 : /* 倒立開始 */ // u1s_trace = 1; /* トレースOFF */ u1s_touritu = 1; /* トレースON */ left = 0; right = 0; f4g_forward = 0; // balance_init(); /* balance APIの初期化 */ // nxt_motor_set_count(PORT_MOTOR_L, 0); /* 左モータ・カウント値[°]を0リセット */ // nxt_motor_set_count(PORT_MOTOR_R, 0); /* 右モータ・カウント値[°]を0リセット */ s4t_motor_L2 = nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R2 = nxt_motor_get_count(PORT_MOTOR_R); u2_count= 0; // u1g_seesaw_mode = 1; tail_seesaw = tailControl( 30, status->motorangle_T); if(status->motorangle_T > 10){ u1g_seesaw_ride_mode = 1; tail_seesaw = 0; } break; case 1 : s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2; if(s4t_distance < 30){ u1s_trace = 1; /*トレースの設定*/ f4g_forward = 20; chokushin2(); }else if(s4t_distance < 200){ u1s_trace = 1; /*トレースの設定*/ f4g_forward = 40; }else if(status->gyroval > status->gyro_offset + 40 || status->gyroval < status->gyro_offset - 40){ u1s_trace = 1; /*トレースの設定*/ f4g_forward = 40; s4t_motor_L2 = nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R2 = nxt_motor_get_count(PORT_MOTOR_R); u1g_seesaw_ride_mode++; } break; case 2 : /* 段差検知まで */ s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2; if(s4t_distance < 200){ u1s_trace = 1; /*トレースの設定*/ f4g_forward = 40; }else{ f4g_forward = 20; if(f4g_turn < 3 && f4g_turn > -3){ u2_count++; }else{ u2_count = 0; } } if(u2_count > 20 && f4g_turn < 2 && f4g_turn > -2){ u1g_seesaw_ride_mode++; u2_count = 0; f4g_turn = 0; } break; case 3: u1s_trace = 0; f4g_forward = 30; chokushin2(); if(status->gyroval > status->gyro_offset + 40 || status->gyroval < status->gyro_offset - 40){ u1g_seesaw_ride_mode++; f4g_forward = 100; s4t_motor_L2 = nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R2 = nxt_motor_get_count(PORT_MOTOR_R); u2_count = 0; u1s_trace = 0; u2_gyro_temp = robot.robotSensor.GYRO.gyroOffsetValue - 5; } break; case 4 : /* 検知後ある一定以上上る */ s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2; if(s4t_distance < 50){ //ベース100 robot.robotSensor.GYRO.gyroOffsetValue = u2_gyro_temp; f4g_forward = 80; chokushin2(); }else if(s4t_distance < 250){ //べーす300 // robot.robotSensor.GYRO.gyroOffsetValue = u2_gyro_temp; f4g_forward = 20; chokushin2(); }else{ u2_count = 0; f4g_forward = 0; chokushin2(); u1g_seesaw_ride_mode = 6; } break; case 5 : /* Bluetoothが来るまで待機 */ f4g_forward = 0; chokushin2(); u4_recieve = ecrobot_read_bt_packet( receive_data, 90 ); if ( u4_recieve > 0 ) { ecrobot_sound_tone(100, 100, 50); u2_count = 0; f4g_forward = 0; chokushin2(); u1g_seesaw_ride_mode++; } break; case 6 : /* Bluetoothが来たから上る */ if(u2_count < 250){ f4g_forward = 40; chokushin2(); u2_count++; }else{ if(status->gyroval > status->gyro_offset + 40 || status->gyroval < status->gyro_offset - 40){ u1g_seesaw_ride_mode++; s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R); u2_count = 0; }else{ f4g_forward = 40; chokushin2(); } } break; case 7 : /* 段差検知 */ s4t_distance = s4t_motor_L1 - s4t_motor_L3 + s4t_motor_R1 - s4t_motor_R3; if(s4t_distance < 100){ f4g_forward = 60; chokushin2(); }else{ f4g_forward = 0; chokushin2(); u2_count++; if(u2_count > 500){ s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R); u1g_seesaw_ride_mode++; u2_count = 0; } } break; case 8 : /* ライン復帰 */ if(s4t_motor_L1 - s4t_motor_L3 < 250){ if(status->lightval > GRAY_WHITE){ f4g_forward = 20; f4g_turn = f4g_forward; u1g_seesaw_ride_mode = 11; ecrobot_sound_tone(100, 100, 50); }else{ f4g_forward = 20; f4g_turn = f4g_forward; } }else{ f4g_turn = 0; u1g_seesaw_ride_mode = 9; } break; case 9 : /* 右側に居る */ if( s4t_motor_L1 - s4t_motor_L3 > -100){ f4g_forward = -20; f4g_turn = f4g_forward; }else{ // f4g_turn = 0; // f4g_forward = 0; u1g_seesaw_ride_mode = 10; } break; case 10: f4g_forward = 30; chokushin2(); if(status->lightval > GRAY_WHITE){ f4g_turn = 0; f4g_forward = 0; u1s_trace = 1; f4g_forward = 20; s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R); u1g_seesaw_ride_mode = 13; } break; case 11 : /* ライン復帰 */ if((status->lightval > GRAY_WHITE - 30) && (u2_count == 0)){ f4g_turn = f4g_forward; //ecrobot_sound_tone(1000, 1000, 50); }else{ f4g_forward = 0; f4g_turn = -40; u2_count = 1; if((s4t_motor_L1 - s4t_motor_L3) - (s4t_motor_R1 - s4t_motor_R3) < -100 ){ f4g_forward = 0; //f4g_turn = 0; u1g_seesaw_ride_mode = 12; } } break; case 12: f4g_forward = 30; chokushin2(); if(status->lightval > GRAY_WHITE){ f4g_turn = 0; f4g_forward = 0; u1s_trace = 1; f4g_forward = 20; u1g_seesaw_ride_mode = 13; s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R); } break; case 13 : u1s_trace = 1; f4g_forward = 20; s4t_distance = s4t_motor_L1 - s4t_motor_L3 + s4t_motor_R1 - s4t_motor_R3; if(s4t_distance > 300){ u1g_seesaw_ride_mode = 14; } break; case 14 : u1s_trace = 1; f4g_forward = 20; break; } if (u1s_trace == 1) { f4t_yp = (f4)(GRAY_WHITE - status->lightval ); f4t_yd = (f4t_yp - f4s_last_yp) / 0.004; f4s_last_yp = f4t_yp; f4g_turn = (f4)((f4)TRACE_P * f4t_yp) + (f4)((f4)TRACE_D * f4t_yd); // f4g_turn = -f4g_turn; /* 旋回方向決定 */ if(f4g_turn > 50){ f4g_turn = 50; }else if(f4g_turn < -50){ f4g_turn = -50; }else{ f4g_turn = f4g_turn; } f4g_turn = -f4g_turn; } if (u1s_touritu == 1) { balance_control( (f4)f4g_forward, /* 前後進命令(+:前進, -:後進) */ (f4)f4g_turn, /* 旋回命令(+:右旋回, -:左旋回) */ // (f4)-6, /* 旋回命令(+:右旋回, -:左旋回) */ (f4)status->gyroval, /* ジャイロセンサ値 */ (f4)status->gyro_offset, /* ジャイロセンサオフセット値 */ (f4)status->motorangle_L, /* 左モータ回転角度[deg] */ (f4)status->motorangle_R, /* 右モータ回転角度[deg] */ (f4)status->batteryval, /* バッテリ電圧[mV] */ &left, /* 左モータPWM出力値 */ &right /* 右モータPWM出力値 */ ); } setMotorVal( left , right , tail_seesaw ); } /* user logic end */ }
/** * NOTE: The technique is not the same as that used in TinyVM. * The return value indicates the impact of the call on the VM * system. EXEC_CONTINUE normal return the system should return to the return * address provided by the VM. EXEC_RUN The call has modified the value of * VM PC and this should be used to restart execution. EXEC_RETRY The call * needs to be re-tried (typically for a GC failure), all global state * should be left intact, the PC has been set appropriately. * */ int dispatch_native(TWOBYTES signature, STACKWORD * paramBase) { STACKWORD p0 = paramBase[0]; switch (signature) { case wait_4_5V: return monitor_wait((Object *) word2ptr(p0), 0); case wait_4J_5V: return monitor_wait((Object *) word2ptr(p0), ((int)paramBase[1] > 0 ? 0x7fffffff : paramBase[2])); case notify_4_5V: return monitor_notify((Object *) word2ptr(p0), false); case notifyAll_4_5V: return monitor_notify((Object *) word2ptr(p0), true); case start_4_5V: // Create thread, allow for instruction restart return init_thread((Thread *) word2ptr(p0)); case yield_4_5V: schedule_request(REQUEST_SWITCH_THREAD); break; case sleep_4J_5V: sleep_thread(((int)p0 > 0 ? 0x7fffffff : paramBase[1])); schedule_request(REQUEST_SWITCH_THREAD); break; case getPriority_4_5I: push_word(get_thread_priority((Thread *) word2ptr(p0))); break; case setPriority_4I_5V: { STACKWORD p = (STACKWORD) paramBase[1]; if (p > MAX_PRIORITY || p < MIN_PRIORITY) return throw_new_exception(JAVA_LANG_ILLEGALARGUMENTEXCEPTION); else set_thread_priority((Thread *) word2ptr(p0), p); } break; case currentThread_4_5Ljava_3lang_3Thread_2: push_ref(ptr2ref(currentThread)); break; case interrupt_4_5V: interrupt_thread((Thread *) word2ptr(p0)); break; case interrupted_4_5Z: { JBYTE i = currentThread->interruptState != INTERRUPT_CLEARED; currentThread->interruptState = INTERRUPT_CLEARED; push_word(i); } break; case isInterrupted_4_5Z: push_word(((Thread *) word2ptr(p0))->interruptState != INTERRUPT_CLEARED); break; case join_4_5V: join_thread((Thread *) word2ptr(p0), 0); break; case join_4J_5V: join_thread((Thread *) word2obj(p0), paramBase[2]); break; case halt_4I_5V: schedule_request(REQUEST_EXIT); break; case shutdown_4_5V: shutdown_program(false); break; case currentTimeMillis_4_5J: push_word(0); push_word(systick_get_ms()); break; case readSensorValue_4I_5I: push_word(sp_read(p0, SP_ANA)); break; case setPowerTypeById_4II_5V: sp_set_power(p0, paramBase[1]); break; case freeMemory_4_5J: push_word(0); push_word(getHeapFree()); break; case totalMemory_4_5J: push_word(0); push_word(getHeapSize()); break; case floatToRawIntBits_4F_5I: // Fall through case intBitsToFloat_4I_5F: push_word(p0); break; case doubleToRawLongBits_4D_5J: // Fall through case longBitsToDouble_4J_5D: push_word(p0); push_word(paramBase[1]); break; case drawString_4Ljava_3lang_3String_2II_5V: { String *p = (String *)word2obj(p0); Object *charArray; if (!p) return throw_new_exception(JAVA_LANG_NULLPOINTEREXCEPTION); charArray = (Object *) word2ptr(get_word_4_ns(fields_start(p))); if (!charArray) return throw_new_exception(JAVA_LANG_NULLPOINTEREXCEPTION); display_goto_xy(paramBase[1], paramBase[2]); display_jstring(p); } break; case drawInt_4III_5V: display_goto_xy(paramBase[1], paramBase[2]); display_int(p0, 0); break; case drawInt_4IIII_5V: display_goto_xy(paramBase[2], paramBase[3]); display_int(p0, paramBase[1]); break; case asyncRefresh_4_5V: display_update(); break; case clear_4_5V: display_clear(0); break; case getDisplay_4_5_1B: push_word(display_get_array()); break; case setAutoRefreshPeriod_4I_5I: push_word(display_set_auto_update_period(p0)); break; case getRefreshCompleteTime_4_5I: push_word(display_get_update_complete_time()); break; case bitBlt_4_1BIIII_1BIIIIIII_5V: { Object *src = word2ptr(p0); Object *dst = word2ptr(paramBase[5]); display_bitblt((byte *)(src != NULL ?jbyte_array(src):NULL), paramBase[1], paramBase[2], paramBase[3], paramBase[4], (byte *)(dst!=NULL?jbyte_array(dst):NULL), paramBase[6], paramBase[7], paramBase[8], paramBase[9], paramBase[10], paramBase[11], paramBase[12]); break; } case getSystemFont_4_5_1B: push_word(display_get_font()); break; case setContrast_4I_5V: nxt_lcd_set_pot(p0); break; case getBatteryStatus_4_5I: push_word(battery_voltage()); break; case getButtons_4_5I: push_word(buttons_get()); break; case getTachoCountById_4I_5I: push_word(nxt_motor_get_count(p0)); break; case controlMotorById_4III_5V: nxt_motor_set_speed(p0, paramBase[1], paramBase[2]); break; case resetTachoCountById_4I_5V: nxt_motor_set_count(p0, 0); break; case i2cEnableById_4II_5V: if (i2c_enable(p0, paramBase[1]) == 0) return EXEC_RETRY; else break; case i2cDisableById_4I_5V: i2c_disable(p0); break; case i2cStatusById_4I_5I: push_word(i2c_status(p0)); break; case i2cStartById_4II_1BIII_5I: { Object *p = word2obj(paramBase[2]); JBYTE *byteArray = p ? jbyte_array(p) + paramBase[3] : NULL; push_word(i2c_start(p0, paramBase[1], (U8 *)byteArray, paramBase[4], paramBase[5])); } break; case i2cCompleteById_4I_1BII_5I: { Object *p = word2ptr(paramBase[1]); JBYTE *byteArray = p ? jbyte_array(p) + paramBase[2] : NULL; push_word(i2c_complete(p0, (U8 *)byteArray, paramBase[3])); } break; case playFreq_4III_5V: sound_freq(p0,paramBase[1], paramBase[2]); break; case btGetBC4CmdMode_4_5I: push_word(bt_get_mode()); break; case btSetArmCmdMode_4I_5V: if (p0 == 0) bt_set_arm7_cmd(); else bt_clear_arm7_cmd(); break; case btSetResetLow_4_5V: bt_set_reset_low(); break; case btSetResetHigh_4_5V: bt_set_reset_high(); break; case btWrite_4_1BII_5I: { Object *p = word2ptr(p0); byte *byteArray = (byte *) jbyte_array(p); push_word(bt_write(byteArray, paramBase[1], paramBase[2])); } break; case btRead_4_1BII_5I: { Object *p = word2ptr(p0); byte *byteArray = (byte *) jbyte_array(p); push_word(bt_read(byteArray, paramBase[1], paramBase[2])); } break; case btPending_4_5I: { push_word(bt_event_check(0xffffffff)); } break; case btEnable_4_5V: if (bt_enable() == 0) return EXEC_RETRY; else break; case btDisable_4_5V: bt_disable(); break; case usbRead_4_1BII_5I: { Object *p = word2ptr(p0); byte *byteArray = (byte *) jbyte_array(p); push_word(udp_read(byteArray,paramBase[1], paramBase[2])); } break; case usbWrite_4_1BII_5I: { Object *p = word2ptr(p0); byte *byteArray = (byte *) jbyte_array(p); push_word(udp_write(byteArray,paramBase[1], paramBase[2])); } break; case usbStatus_4_5I: { push_word(udp_event_check(0xffffffff)); } break; case usbEnable_4I_5V: { udp_enable(p0); } break; case usbDisable_4_5V: { udp_disable(); } break; case usbReset_4_5V: udp_reset(); break; case usbSetSerialNo_4Ljava_3lang_3String_2_5V: { byte *p = word2ptr(p0); int len; Object *charArray = (Object *) word2ptr(get_word_4_ns(fields_start(p))); len = get_array_length(charArray); udp_set_serialno((U8 *)jchar_array(charArray), len); } break; case usbSetName_4Ljava_3lang_3String_2_5V: { byte *p = word2ptr(p0); int len; Object *charArray = (Object *) word2ptr(get_word_4_ns(fields_start(p))); len = get_array_length(charArray); udp_set_name((U8 *)jchar_array(charArray), len); } break; case flashWritePage_4_1BI_5I: { Object *p = word2ptr(p0); unsigned long *intArray = (unsigned long *) jint_array(p); push_word(flash_write_page(intArray,paramBase[1])); } break; case flashReadPage_4_1BI_5I: { Object *p = word2ptr(p0); unsigned long *intArray = (unsigned long *) jint_array(p); push_word(flash_read_page(intArray,paramBase[1])); } break; case flashExec_4II_5I: push_word(run_program((byte *)(&FLASH_BASE[(p0*FLASH_PAGE_SIZE)]), paramBase[1])); break; case playSample_4IIIII_5V: sound_play_sample(((unsigned char *) &FLASH_BASE[(p0*FLASH_PAGE_SIZE)]) + paramBase[1],paramBase[2],paramBase[3],paramBase[4]); break; case playQueuedSample_4_1BIIII_5I: push_word(sound_add_sample((U8 *)jbyte_array(word2obj(p0)) + paramBase[1],paramBase[2],paramBase[3],paramBase[4])); break; case getTime_4_5I: push_word(sound_get_time()); break; case getDataAddress_4Ljava_3lang_3Object_2_5I: if (is_array(word2obj(p0))) push_word (ptr2word ((byte *) array_start(word2ptr(p0)))); else push_word (ptr2word ((byte *) fields_start(word2ptr(p0)))); break; case getObjectAddress_4Ljava_3lang_3Object_2_5I: push_word(p0); break; case gc_4_5V: // Restartable garbage collection return garbage_collect(); case shutDown_4_5V: shutdown(); // does not return case boot_4_5V: display_clear(1); while (1) nxt_avr_firmware_update_mode(); // does not return case arraycopy_4Ljava_3lang_3Object_2ILjava_3lang_3Object_2II_5V: return arraycopy(word2ptr(p0), paramBase[1], word2ptr(paramBase[2]), paramBase[3], paramBase[4]); case executeProgram_4I_5V: // Exceute program, allow for instruction re-start return execute_program(p0); case setDebug_4_5V: set_debug(word2ptr(p0)); break; case eventOptions_4II_5I: { byte old = debugEventOptions[p0]; debugEventOptions[p0] = (byte)paramBase[1]; push_word(old); } break; case suspendThread_4Ljava_3lang_3Object_2_5V: suspend_thread(ref2ptr(p0)); break; case resumeThread_4Ljava_3lang_3Object_2_5V: resume_thread(ref2ptr(p0)); break; case getProgramExecutionsCount_4_5I: push_word(gProgramExecutions); break; case getFirmwareRevision_4_5I: push_word((STACKWORD) getRevision()); break; case getFirmwareRawVersion_4_5I: push_word((STACKWORD) VERSION_NUMBER); break; case hsEnable_4II_5V: { if (hs_enable((int)p0, (int)paramBase[1]) == 0) return EXEC_RETRY; } break; case hsDisable_4_5V: { hs_disable(); } break; case hsWrite_4_1BII_5I: { Object *p = word2ptr(p0); byte *byteArray = (byte *) jbyte_array(p); push_word(hs_write(byteArray, paramBase[1], paramBase[2])); } break; case hsRead_4_1BII_5I: { Object *p = word2ptr(p0); byte *byteArray = (byte *) jbyte_array(p); push_word(hs_read(byteArray, paramBase[1], paramBase[2])); } break; case hsPending_4_5I: { push_word(hs_pending()); } break; case hsSend_4BB_1BII_1C_5I: { Object *p = word2ptr(paramBase[2]); U8 *data = (U8 *)jbyte_array(p); p = word2ptr(paramBase[5]); U16 *crc = (U16 *)jchar_array(p); push_word(hs_send((U8) p0, (U8)paramBase[1], data, paramBase[3], paramBase[4], crc)); } break; case hsRecv_4_1BI_1CI_5I: { Object *p = word2ptr(p0); U8 *data = (U8 *)jbyte_array(p); p = word2ptr(paramBase[2]); U16 *crc = (U16 *)jchar_array(p); push_word(hs_recv(data, paramBase[1], crc, paramBase[3])); } break; case getUserPages_4_5I: push_word(FLASH_MAX_PAGES - flash_start_page); break; case setVMOptions_4I_5V: gVMOptions = p0; break; case getVMOptions_4_5I: push_word(gVMOptions); break; case isAssignable_4II_5Z: push_word(is_assignable(p0, paramBase[1])); break; case cloneObject_4Ljava_3lang_3Object_2_5Ljava_3lang_3Object_2: { Object *newObj = clone((Object *)ref2obj(p0)); if (newObj == NULL) return EXEC_RETRY; push_word(obj2ref(newObj)); } break; case memPeek_4III_5I: push_word(mem_peek(p0, paramBase[1], paramBase[2])); break; case memCopy_4Ljava_3lang_3Object_2IIII_5V: mem_copy(word2ptr(p0), paramBase[1], paramBase[2], paramBase[3], paramBase[4]); break; case memGetReference_4II_5Ljava_3lang_3Object_2: push_word(mem_get_reference(p0, paramBase[1])); break; case setSensorPin_4III_5V: sp_set(p0, paramBase[1], paramBase[2]); break; case getSensorPin_4II_5I: push_word(sp_get(p0, paramBase[1])); break; case setSensorPinMode_4III_5V: sp_set_mode(p0, paramBase[1], paramBase[2]); break; case readSensorPin_4II_5I: push_word(sp_read(p0, paramBase[1])); break; case nanoTime_4_5J: { U64 ns = systick_get_ns(); push_word(ns >> 32); push_word(ns); } break; case createStackTrace_4Ljava_3lang_3Thread_2Ljava_3lang_3Object_2_5_1I: { Object *trace = create_stack_trace((Thread *)ref2obj(p0), ref2obj(paramBase[1])); if (trace == NULL) return EXEC_RETRY; push_word(obj2ref(trace)); } break; case registerEvent_4_5I: push_word(register_event((NXTEvent *) ref2obj(p0))); break; case unregisterEvent_4_5I: push_word(unregister_event((NXTEvent *) ref2obj(p0))); break; case changeEvent_4II_5I: push_word(change_event((NXTEvent *) ref2obj(p0), paramBase[1], paramBase[2])); break; case isInitialized_4I_5Z: push_word(is_initialized_idx(p0)); break; case allocate_4II_5Ljava_3lang_3Object_2: { Object *allocated; if(paramBase[1]>0){ allocated=new_single_array(p0,paramBase[1]); }else{ allocated=new_object_for_class(p0); } if(allocated == NULL) return EXEC_RETRY; push_word(obj2ref(allocated)); } break; case memPut_4IIII_5V: store_word_ns((byte *)(memory_base[p0] + paramBase[1]), paramBase[2],paramBase[3]); break; case notifyEvent_4ILjava_3lang_3Thread_2_5Z: push_word(debug_event(paramBase[1], NULL, (Thread*) ref2obj(paramBase[2]), 0, 0, 0, 0)); break; case setThreadRequest_4Ljava_3lang_3Thread_2Llejos_3nxt_3debug_3SteppingRequest_2_5V: { Thread *th = (Thread*) ref2obj(p0); th->debugData = (REFERENCE) paramBase[1]; // currently we only get stepping requests if(paramBase[1]) th->flags |= THREAD_STEPPING; else th->flags &= ~THREAD_STEPPING; } break; case isStepping_4Ljava_3lang_3Thread_2_5Z: { Thread *th = (Thread*) ref2obj(p0); push_word(is_stepping(th)); } break; case setBreakpointList_4_1Llejos_3nxt_3debug_3Breakpoint_2I_5V: breakpoint_set_list((Breakpoint**) array_start(p0), paramBase[1]); break; case enableBreakpoint_4Llejos_3nxt_3debug_3Breakpoint_2Z_5V: breakpoint_enable((Breakpoint*) word2ptr(p0), (boolean) paramBase[1]); break; case firmwareExceptionHandler_4Ljava_3lang_3Throwable_2II_5V: firmware_exception_handler((Throwable *)p0, paramBase[1], paramBase[2]); break; case exitThread_4_5V: currentThread->state = DEAD; schedule_request(REQUEST_SWITCH_THREAD); break; case updateThreadFlags_4Ljava_3lang_3Thread_2II_5I: ((Thread *)p0)->flags |= paramBase[1]; ((Thread *)p0)->flags &= ~paramBase[2]; //printf("m %x %d\n", p0, ((Thread *)p0)->flags); push_word(((Thread *)p0)->flags); break; default: return throw_new_exception(JAVA_LANG_NOSUCHMETHODERROR); } return EXEC_CONTINUE; }