int UI::courseSelect(){
int course = -1;
display_goto_xy(4, 1);
while(1){
if(ecrobot_is_RUN_button_pressed()){
course = course * (-1);
if(course == 1){
display_string(" IN COURSE");
display_goto_xy(4, 1);
}
else{
display_string("OUT COURSE");
display_goto_xy(4, 1);
}
}
if(ecrobot_is_ENTER_button_pressed()){
display_goto_xy(6, 3);
display_string("READY");
break;
}
display_update();
// 500msecウェイトする
systick_wait_ms(500);
}
display_update();
return course;
}
void TestIRSeeker(U8 port_id)
{
S8 data[6];
ecrobot_get_ir_seeker(port_id, data);
display_goto_xy(0, 0);
display_string("IR SEEKER TEST");
display_goto_xy(0, 1);
display_string("DIR: ");
display_int(data[0], 0);
display_goto_xy(0, 2);
display_string("INT1/2: ");
display_int(data[1], 0);
display_int(data[2], 5);
display_goto_xy(0, 3);
display_string("INT3/4: ");
display_int(data[3], 0);
display_int(data[4], 5);
display_goto_xy(0, 4);
display_string("INT5: ");
display_int(data[5], 0);
}
void show(int distance)
{
display_goto_xy(0, 0);
display_string("DISTANCE :");
display_goto_xy(0, 1);
display_unsigned(distance,10);
display_update();
}
void TestCompassSensor(U8 port_id)
{
display_goto_xy(0, 0);
display_string("COMPASS TEST");
display_goto_xy(0, 1);
display_string("HEADING: ");
display_int(ecrobot_get_compass_sensor(port_id), 0);
}
void display_currentNode(){
//display_clear(1);
display_goto_xy(5,6);
display_int((int)previousNode[0],1);
display_goto_xy(8,6);
display_int((int)previousNode[1],1);
display_goto_xy(6,5);
display_int(directionOffset,1);
display_goto_xy(6,7);
display_int((int)mazeStorage[previousNode[0]][previousNode[1]],1);
display_update();
}
void display_show_string(char* string,int x,int y){
display_goto_xy(x, y);
display_string(string);
display_update();
}
void disp(int row, char *str, int val)
{
if (row == 0) display_clear(0);
display_goto_xy(0, row);
display_string(str);
display_int(val, 0);
if (row == 7) display_update();
}
// 入力を待つ
void UI::waitStart(float angle){
while(1){
tail->control(angle);
if(touchSensor->isPressed()){
display_goto_xy(6, 5);
display_string("GO !!");
display_update();
break;
}
if(blueTooth->isReceived() == 1){
display_goto_xy(6, 5);
display_string("GO !!");
display_update();
break;
}
}
}
void myPrintln(const char *s)
{
static int y = 0;
display_goto_xy(0,y);
display_string(s);
display_update();
y++;
}
//*****************************************************************************
// 関数名 : 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 TestColorSensor(U8 port_id)
{
S16 data[3];
ecrobot_get_color_sensor(port_id, data);
display_goto_xy(0, 0);
display_string("COLOR TEST");
display_goto_xy(0, 1);
display_string("R: ");
display_int(data[0], 0);
display_goto_xy(0, 2);
display_string("G: ");
display_int(data[1], 0);
display_goto_xy(0, 3);
display_string("B: ");
display_int(data[2], 0);
}
void __nxtAssert(const char *file, int line, const char *exp)
{
display_clear(0);
display_goto_xy(0, 0);
display_string("assert");
display_goto_xy(0, 1);
display_string("File: ");
display_string(file);
display_goto_xy(0, 2);
display_string("Line: ");
display_int(line,0);
display_goto_xy(0, 3);
display_string(exp);
display_update();
#ifdef NXT_WARN_ASSERT
display_goto_xy(0, 3);
display_string("Press ENTER");
#endif
while(true)
{
#ifdef NXT_WARN_ASSERT
if(ecrobot_is_ENTER_button_pressed())
{
display_clear(0);
display_goto_xy(0, 0);
return;
}
#endif
}
}
void print(const char* intern_message)
{
static int y = 0;
if(y == 8)
{
y = 0;
display_clear(0);
}
display_goto_xy(0,y);
display_string(intern_message);
display_update();
y++;
}
void flush()
{
if (len > 0)
{
#ifdef LCD
static int line = 0;
buf[len] = '\0';
display_goto_xy( 0, line);
display_string(" ");
display_goto_xy( 0, line);
display_string((char *)buf+2);
line = (line + 1) & 7;
display_goto_xy( 0, line);
display_string(" ");
display_update();
//systick_wait_ms(10000);
#else
buf[0] = len;
buf[1] = 0;
udp_rconsole(buf, len + 2);
#endif
}
len = 0;
}
void display_values(void) {
char *message = NULL;
U8 line = 0;
display_goto_xy(0, line++);
message = "size:";
display_string(message);
display_int(sizeLCD, 4);
display_goto_xy(0, line++);
message = "area:";
display_string(message);
display_int(areaLCD, 5);
display_goto_xy(0, line++);
message = "pos:";
display_string(message);
display_int(xLCD, 4);
display_goto_xy(0, line++);
message = "speed:";
display_string(message);
display_int(speedLCD, 4);
display_goto_xy(0, line++);
message = "dev speed:";
display_string(message);
display_int(devspeedLCD, 4);
display_goto_xy(0, line++);
message = "dir dev:";
display_string(message);
display_int(diradjLCD, 4);
display_goto_xy(0, line++);
message = "left motor:";
display_string(message);
display_int(lmotLCD, 4);
display_goto_xy(0, line++);
message = "right motor:";
display_string(message);
display_int(rmotLCD, 4);
display_update();
}
void display_bios_status(int data, int max_data)
{
static U8 first_flag = 1;
if (first_flag)
{
display_clear(0);
display_goto_xy(0, 0);
display_string(VERSION);
display_goto_xy(0, 1);
display_string("================");
}
display_goto_xy(0, 2);
display_string("BATT:");
display_unsigned(battery_voltage()/100, 0);
if (first_flag)
{
display_goto_xy(0, 3);
display_string("UPLOAD: READY ");
first_flag = 0;
}
else
{
if (data >= UPLOAD_IN_PROGRESS)
{
display_goto_xy(0, 3);
display_string("UPLOAD: PROGRESS");
display_progress_bar(4, data, max_data);
}
else if (data == UPLOAD_FAILED)
{
display_goto_xy(0, 3);
display_string("UPLOAD: FAILED ");
}
else if (data == UPLOAD_FINISHED)
{
display_goto_xy(0, 3);
display_string("UPLOAD: FINISHED");
/* clear progress bar */
display_progress_bar(4, 0, 100);
}
}
display_update();
}
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();
}
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;
}
}
}
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){ //べーす３００
// 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;
}
void update_counter(){
display_goto_xy(7,13);
display_int((int)getNextCalled,3);
display_update();
}
void disp(int row, char *str, int val)
{
display_goto_xy(0, row);
display_string(str);
display_int(val, 0);
}
//implemetation of functions
int start_finding(int start_x, int start_y)
{
int inter = 0;
int token = 0;
int cur_x = start_x, cur_y = start_y;
int dir = SOUTH;
int ppath = -1;
int npop = 0; //how many points should be poped
struct POINT *cur_p = NULL;
struct POINT *tmp_p = NULL;
int ret = 0;
#ifdef DEBUG
inter = Robot_GetIntersections();
#else
inter = get_intersection();
#endif
cur_p = mark_point(cur_x, cur_y, inter);
dir = get_direction(cur_p);
#ifdef DEBUG
printf("start point: ");
print_point(cur_p);
printf("\n");
#endif
while(token < TOKEN_COUNT)
{
#ifdef DEBUG
//inter = Robot_GetIntersections();
//print_intersection(inter);
#endif
if(points[cur_x][cur_y].detected == 0)
cur_p = mark_point(cur_x, cur_y, inter);
else
cur_p = &points[cur_x][cur_y];
push(cur_p);
//print_stack();
if(dir = get_direction(cur_p))
{
//update current point
switch(dir)
{
case EAST:
cur_x += 1;
break;
case SOUTH:
cur_y -= 1;
break;
case WEST:
cur_x -= 1;
break;
case NORTH:
cur_y += 1;
break;
}
#ifdef DEBUG
print_direction(cur_p, dir);
ret = aud_move(cur_p, dir);
#else
//move one step
display_clear(0);
display_goto_xy(0, 0);
display_int(cur_p->x, 2);
display_goto_xy(3, 0);
display_int(cur_p->y, 2);
display_goto_xy(7, 0);
display_int(cur_x, 2);
display_goto_xy(10, 0);
display_int(cur_y, 2);
display_goto_xy(0, 1);
display_int(g_dir, 3);
display_goto_xy(5, 1);
display_int(dir, 3);
display_goto_xy(0, 2);
display_int(cur_p->inter&0xF0, 3);
display_update();
ret = move(cur_x, cur_y);
#endif
#ifdef DEBUG
inter = Robot_GetIntersections();
#else
inter = get_intersection();
#endif
cur_p = mark_point(cur_x, cur_y, inter);
#ifdef DEBUG
print_point(cur_p);
#endif
if(ret == ROBOT_SUCCESS)
{
#ifndef DEBUG
#endif
}
else if(ret == ROBOT_TOKENFOUND)
{
tmp_p = &points[cur_x][cur_y];
if(tmp_p->has_token == 0)
{
tmp_p->has_token = 1;
token++;
#ifdef DEBUG
printf("[%d. token]\n", token);
#endif
}
else
{
#ifdef DEBUG
printf("[not a new token]\n");
#endif
}
if(token == TOKEN_COUNT)
{
//all token were found, go back to start point
#ifdef DEBUG
printf("going back to start point......\n");
#endif
push(cur_p);
ppath = find_shortest_path(cur_p->x, cur_p->y, START_X, START_Y);
if(ppath)
{
//going back to last open point
ppath--;
while(ppath >= 0)
{
tmp_p = shortest_path[ppath];
dir = calc_direction(cur_p->x, cur_p->y, tmp_p->x, tmp_p->y);
#ifdef DEBUG
print_point(tmp_p);
printf("\n");
ROBOT_MOVE(tmp_p->x, tmp_p->y);
#else
display_clear(0);
display_goto_xy(0, 0);
display_int(cur_p->x, 2);
display_goto_xy(3, 0);
display_int(cur_p->y, 2);
display_goto_xy(7, 0);
display_int(tmp_p->x, 2);
display_goto_xy(10, 0);
display_int(tmp_p->y, 2);
display_goto_xy(0, 1);
display_int(g_dir, 3);
display_goto_xy(5, 1);
display_int(dir, 3);
display_goto_xy(0, 2);
display_int(cur_p->inter&0xF0, 3);
display_update();
move(tmp_p->x, tmp_p->y);
#endif
cur_p = tmp_p;
ppath--;
}
//delete the path in stack
pop(npop + 1);
cur_p = tmp_p;
cur_x = cur_p->x;
cur_y = cur_p->y;
}
#ifdef DEBUG
printf("task finished!\n");
#else
beep();
#endif
break;
}
}
else
{
#ifdef DEBUG
printf("move failed!\n");
#endif
}
}
else
{
//there is no ways forward, go back to nearest open point
tmp_p = get_last_open_point();
npop = stack_pointer - get_stack_index(tmp_p->x, tmp_p->y);
#ifdef DEBUG
printf("going back to (%d, %d)\n", tmp_p->x, tmp_p->y);
#endif
if(tmp_p)
{
if((tmp_p->x == START_X) && (tmp_p->y == START_Y) && !IS_OPEN_POINT(points[tmp_p->x][tmp_p->y]))
{
#ifdef DEBUG
return 0;
#else
stop_robot();
beep();
return 0;
#endif
}
ppath = find_shortest_path(cur_p->x, cur_p->y, tmp_p->x, tmp_p->y);
if(ppath)
{
//going back to last open point
ppath--;
while(ppath >= 0)
{
tmp_p = shortest_path[ppath];
dir = calc_direction(cur_p->x, cur_p->y, tmp_p->x, tmp_p->y);
#ifdef DEBUG
ROBOT_MOVE(tmp_p->x, tmp_p->y);
#else
display_clear(0);
display_goto_xy(0, 0);
display_int(cur_p->x, 2);
display_goto_xy(3, 0);
display_int(cur_p->y, 2);
display_goto_xy(7, 0);
display_int(tmp_p->x, 2);
display_goto_xy(10, 0);
display_int(tmp_p->y, 2);
display_goto_xy(0, 1);
display_int(g_dir, 3);
display_goto_xy(5, 1);
display_int(dir, 3);
display_goto_xy(0, 2);
display_int(cur_p->inter&0xF0, 3);
display_update();
move(tmp_p->x, tmp_p->y);
#endif
cur_p = tmp_p;
ppath--;
}
//delete the path in stack
pop(npop + 1);
cur_p = tmp_p;
cur_x = cur_p->x;
cur_y = cur_p->y;
}
else
{
//was already at every point and back to start point
//task should be ended
//that means, not enough token can be found
#ifdef DEBUG
printf("task ended without enough token were found.\n");
#endif
break;
}
}
}
#ifdef DEBUG
printf("\n");
#endif
}
return 0;
}
SINT main(void)
{
#ifdef NO_RUN_ENTER_STOP_EXIT
init_OS_flag(); /* this should be called before device init */
nxt_device_init();
ecrobot_initDeviceStatus(); // added 10/28/2010 to fix a bug by tchikama
ecrobot_init_nxtstate();
if (execution_mode() == EXECUTED_FROM_FLASH)
{
/*
* Call buttons_get() because ecrobot_get_button_state() has button bouncer.
* The button bouncer requires multiple periodical calls to make it work, but
* in this case, only single call (no while loop), so buttons_get is called.
*/
if ((buttons_get() & 0x0F) == (ENTER_PRESSED | STOP_PRESSED))
{
/* set flash request and shut down the NXT
* at the next start, NXT BIOS will be executed.
*/
display_clear(0);
display_goto_xy(0, 0);
display_string("PWR ON: NXT BIOS");
display_update();
systick_wait_ms(1000);
set_flash_request();
display_clear(1);
systick_wait_ms(10);
nxt_lcd_power_down(); /* reset LCD hardware */
systick_wait_ms(10);
while(1)
{
nxt_avr_power_down();
}
}
}
/* device init should be called prior to running the application */
ecrobot_device_initialize();
ecrobot_setDeviceInitialized();
nxt_motor_set_count(NXT_PORT_A, 0);
nxt_motor_set_count(NXT_PORT_B, 0);
nxt_motor_set_count(NXT_PORT_C, 0);
cpp_constructor();
display_clear(1);
systick_wait_ms(10);
#ifdef NXT_JSP
interrupts_get_and_disable();
#else
disable_int(); /* set_OS_flag and Start OS have to be atomic */
#endif
set_OS_flag(); /* this shoud be called before starting OS */
#ifdef NXT_JSP
lejos_osek_run(); /* start TOPPERS JSP */
#else
StartOS(1); /* start TOPPERS OSEK */
#endif
/* never reached here */
#else
/*
* Default start up sequence
*/
U32 st;
U32 last_act_time = 0;
U32 flash_req_cnt = 0;
init_OS_flag(); /* this should be called before device init */
nxt_device_init();
ecrobot_initDeviceStatus(); // added 10/28/2010 to fix a bug by tchikama
ecrobot_init_nxtstate();
show_splash_screen();
show_main_screen();
display_status_bar(1); /* clear status bar */
add_status_info(execution_mode());
display_status_bar(0); /* update status bar */
while(1)
{
/* device init should be called prior to running the application */
ecrobot_device_initialize();
ecrobot_setDeviceInitialized();
/* check the buttons every 10msec */
st = systick_get_ms();
if (st >= last_act_time + 10)
{
last_act_time = st;
ecrobot_poll_nxtstate();
display_status_bar(0);
/*
* executed in FLASH: setup for the application flash
* executed in SRAM: no effect
*/
if ((ecrobot_get_button_state() == (ENTER_PRESSED | STOP_PRESSED)) &&
(execution_mode() == EXECUTED_FROM_FLASH))
{
flash_req_cnt++;
/* keep pusing ENTER + STOP buttons more than 1000msec */
if (flash_req_cnt >= 100)
{
/* set flash request and shut down the NXT
* at the next start, NXT BIOS will be executed.
*/
ecrobot_device_terminate();
set_flash_request();
display_clear(1);
systick_wait_ms(10);
nxt_lcd_power_down(); /* reset LCD hardware */
systick_wait_ms(10);
while(1)
{
nxt_avr_power_down();
}
}
}
else
{
flash_req_cnt = 0;
if ((ecrobot_get_button_state() == EXIT_PRESSED) || (systick_get_ms() > SLEEP_TIME))
{
/* shut down the NXT */
ecrobot_device_terminate();
display_clear(1);
systick_wait_ms(10);
nxt_lcd_power_down(); /* reset LCD hardware */
systick_wait_ms(10);
while(1)
{
nxt_avr_power_down();
}
}
else if (ecrobot_get_button_state() == RUN_PRESSED)
{
nxt_motor_set_count(NXT_PORT_A, 0);
nxt_motor_set_count(NXT_PORT_B, 0);
nxt_motor_set_count(NXT_PORT_C, 0);
cpp_constructor();
display_clear(1);
systick_wait_ms(10);
#ifdef NXT_JSP
interrupts_get_and_disable();
#else
disable_int(); /* set_OS_flag and Start OS have to be atomic */
#endif
set_OS_flag(); /* this shoud be called before starting OS */
#ifdef NXT_JSP
lejos_osek_run(); /* start TOPPERS JSP */
#else
StartOS(1); /* start TOPPERS OSEK */
#endif
/* never reached here */
}
}
}
}
#endif
return 0;
}
