int show_tasks(char *argc){
int i,total=0;
if (strcmp(argc,"-a")==0){
int total=0;
for (i=1; i<NR_TASKS; i++) if (proc_table[i].status!=STOPPED) ++total;
display_string("There are "); display_int(total); display_string(" task!\n");
for (i=1; i<NR_TASKS; i++)
if (proc_table[i].status!=STOPPED) display_task(proc_table+i);
return 0;
} else if (strcmp(argc,"-r")==0){
for (i=1; i<NR_TASKS; i++) if (proc_table[i].status==RUNNING) ++total;
display_string("There are "); display_int(total); display_string(" task RUNNING!\n");
for (i=1; i<NR_TASKS; i++) if (proc_table[i].status==RUNNING) display_task(proc_table+i);
return 0;
} /*else if (strcmp(argc,"-d")==0){
for (i=1; i<NR_TASKS; i++) if (proc_table[i].status==STOPPED) ++total;
display_string("There are "); display_int(total); display_string(" task STOPPED!\n");
for (i=1; i<NR_TASKS; i++) if (proc_table[i].status==STOPPED) display_task(proc_table+i);
return 0;
} */else if (strcmp(argc,"-s")==0){
for (i=1; i<NR_TASKS; i++) if (proc_table[i].status==SLEEPING) ++total;
display_string("There are "); display_int(total); display_string(" task SLEEPING!\n");
for (i=1; i<NR_TASKS; i++) if (proc_table[i].status==SLEEPING) display_task(proc_table+i);
return 0;
} else {
for (i=1; i<NR_TASKS; i++) if (strcmp(proc_table[i].p_name,argc)==0){
display_task(proc_table+i);
return 0;
}
}
return -1;
}
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
exception_handler(int vec_no, int err_code, int eip, int cs, int eflags)
{
int i;
int text_color = 0x74;
char* err_msg[] = {
"#DE divide error",
"#DB reserved",
"-- nmi interrupt",
"#BP breakpoint",
"#OF overflow",
"#BR bound range exceeded",
"#UD invalid opcode (undefined opcode)",
"#NM device not available (no match coprocessor)",
"#DF double fault",
" coprocessor segment overrun (reserved)",
"#TS invalid tss",
"#NP segment not present",
"#SS stack segment fault",
"#GP general protection",
"#PF page fault",
"-- (intel reserved. do not use)",
"#MF x87 FPU floating-point error (math fault)",
"#AC alignment check",
"#MC machine check",
"#XF SIMD floating-point exception"
};
disp_pos = 0;
for (i = 0; i < 80 * 5; ++i)
display_str(" ");
disp_pos = 0;
display_color_str("exception! --> ", text_color);
display_color_str(err_msg[vec_no], text_color);
display_color_str("\n\n", text_color);
display_color_str("eflags: ", text_color);
display_int(eflags);
display_color_str("cs: ", text_color);
display_int(cs);
display_color_str("eip: ", text_color);
display_int(eip);
if (0xffffffff != err_code) {
display_color_str("error code: ", text_color);
display_int(err_code);
}
}
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
}
}
int display_usertype(char *buf) {
char *orig_buf=buf;
uint32_t size = *((uint32_t*)buf);
size = ntohl(size);
buf+=4;
if(strcmp(buf, "NetworkId")==0 || strcmp(buf, "IdentityId")==0 || strcmp(buf, "BufferId")==0 || strcmp(buf, "MsgId")==0) {
printf("%s(", buf);
buf+=strlen(buf)+1;
buf+=display_int(buf, 0);
printf(")");
} else if(strcmp(buf, "Identity")==0) {
buf+=strlen(buf)+1;
buf+=display_map(buf);
} else if(strcmp(buf, "BufferInfo")==0) {
buf+=strlen(buf)+1;
buf+=display_bufferinfo(buf);
} else if(strcmp(buf, "Message")==0) {
buf+=strlen(buf)+1;
buf+=display_message(buf);
} else if(strcmp(buf, "Network::Server")==0) {
buf+=strlen(buf)+1;
buf+=display_map(buf);
} else {
printf(" Usertype('%s') \n", buf);
printf("Unsupported.\n");
exit(0);
}
return buf-orig_buf;
}
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 display_task(PROCESS* process){
display_string("pid = "); display_int(process->pid);
display_string(" name = "); display_string(process->p_name);
display_string(" status = ");
int status=process->status;
if (status == RUNNING) display_string("RUNNING\n"); else
if (status == SLEEPING) display_string("SLEEPING\n");
}
static prg_t basic_prg(void)
{
char cnt = 0;
for (;;)
{
if (HOUR || MINU)
{
if (!--d_s)
{
if (MINU)
MINU--;
else if (HOUR)
{
HOUR--;
MINU = 59;
}
if (!MINU && !HOUR)
next_timer();
if (!power[0])
return PRG_SHOW_STOP;
d_s = 60*4;
}
}
if ((MINU || HOUR) && cnt++ & 0x4)
display_time(HOUR, MINU);
else
display_int(power[0]);
switch (buttons_get())
{
case BTN_UP:
case BTN_UP_LONG:
power_inc(0);
cnt = 0;
break;
case BTN_DOWN:
case BTN_DOWN_LONG:
power_dec(0);
cnt = 0;
break;
case BTN_BOTH:
return PRG_SET_TIMER1;
case BTN_IDLE:
case BTN_UNDEFINED:
case BTN_RESET:
break;
}
sleep();
}
}
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);
}
int display_qvariant(char *buf) {
char *orig_buf=buf;
uint32_t type = *((uint32_t*)buf);
type=ntohl(type);
buf+=4;
char null=*buf;
buf++;
if(null) {
//Nothing to do
}
switch(type) {
case 1:
buf+=display_bool(buf);
break;
case 2:
case 3:
buf+=display_int(buf, type);
break;
case 7:
//UTF16 byte
buf+=display_short(buf);
break;
case 8:
buf+=display_map(buf);
break;
case 9:
buf+=display_list(buf);
break;
case 10:
buf+=display_string(buf);
break;
case 11:
buf+=display_stringlist(buf);
break;
case 12:
buf+=display_bytearray(buf);
break;
case 15:
buf+=display_time(buf);
break;
case 16:
buf+=display_date(buf);
break;
case 127:
//User type !
buf+=display_usertype(buf);
break;
case 133:
buf+=display_short(buf);
break;
default:
printf("Unknown QVariant type: %d\n", type);
exit(-1);
break;
};
return buf-orig_buf;
}
int recieveint_rs485(int *value)
{
totalBytesRead += ecrobot_read_rs485(raw, totalBytesRead, (INT_SIZE+1) - totalBytesRead);
if (totalBytesRead != (INT_SIZE+1))
return 0;
crc_t crcRecieved = (crc_t)raw[0];
U8 rawInt[INT_SIZE];
rawInt[0] = raw[1];
rawInt[1] = raw[2];
rawInt[2] = raw[3];
rawInt[3] = raw[4];
crc_t crc;
crc = crc_init();
crc = crc_update(crc, (unsigned char *)rawInt, INT_SIZE);
crc = crc_finalize(crc);
if (crc != crcRecieved)
{
print_str(0,0,"CRC errors = ");
display_int(++crc_errors, 0);
display_update();
//We reboot the network interface
ecrobot_term_rs485();
ecrobot_init_rs485(NETWORK_SPEED);
totalBytesRead = 0;
return 0;
}
// We convert the raw bytes to an int
*value = 0;
for (int i = INT_SIZE-1; i > -1 ; i--)
{
int temp = 0;
temp += rawInt[i];
temp <<= (8 * i);
*value += temp;
}
totalBytesRead = 0;
// print_clear_line(5);
// display_update();
// display_goto_xy(0,5);
// display_int(*value, 0);
// display_update();
return 1;
}
//*****************************************************************************
// 関数名 : 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 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 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 store_last_ROM(){
char i;
U8 r;
for(i = 7; i > -1; i--)
ROM[i] = ow_data_array[i];
T_time = Global_time;
temper_delay = STORE_delay;
r = store_ROM();
if(r){
display_int((int) r, 0);
}else{
set_display_buf("eff");
}
block_keys = 0;
}
static prg_t set_power2_prg(void)
{
int timeout = TIMEOUT;
while (timeout)
{
if (timeout&1)
display(" ");
else
display_int(power[1]);
sleep();
switch (buttons_get())
{
case BTN_UP:
case BTN_UP_LONG:
timeout = TIMEOUT;
power_inc(1);
break;
case BTN_DOWN:
case BTN_DOWN_LONG:
timeout = TIMEOUT;
power_dec(1);
break;
case BTN_BOTH:
goto Exit;
case BTN_IDLE:
case BTN_UNDEFINED:
case BTN_RESET:
timeout--;
break;
}
}
Exit:
if (power[1])
return PRG_SET_TIMER2;
return PRG_BASIC;
}
void process_command(char* cmd){
char* CMD[10];
memset(CMD,0,10);
int i,pid,total,status;
total=0;
split_by_space(CMD,cmd,&total);
if (total==0) return;
if (strcmp(CMD[0],"clear")==0){
clear_screen();
} else if (strcmp(CMD[0],"run")==0){
if (total<2 || (pid=run(CMD[1]))==-1){
display_string("this program is not exists or already running!\n");
} else{
display_string("run successful ");
display_string("pid = "); display_int(pid);
display_string(" name = "); display_string(CMD[1]);
display_string("\n");
}
} else if (strcmp(CMD[0],"sleep")==0){
if (total<2 || sleep(CMD[1])==-1){
} else{
display_string("Process ");
display_string(CMD[1]);
display_string(" is sleeping now!\n");
}
} else if (strcmp(CMD[0],"kill")==0){
if (total<2 || !is_number(CMD[1])){
display_string("Usage: kill <pid>\n");
} else
if (kill(CMD[1])==-1){
display_string("No such process\n");
} else{
display_string("The process with pid=");
display_string(CMD[1]);
display_string(" was stopped\n");
}
} else if (strcmp(CMD[0],"killall")==0){
if (total<2){
display_string("Usage: killall <name>");
} else{
if (killall(CMD[1])==-1){
display_string("No such process\n");
} else{
display_string("the process with name=");
display_string(CMD[1]);
display_string(" was stopped\n");
}
}
} else if (strcmp(CMD[0],"setp")==0){
if (total<3 || !is_number(CMD[2])){
display_string("Usage: setp <name> <num>\n");
} else if (setp(CMD[1],atoi(CMD[2]))==-1){
display_string("i can't set the priority of ");
display_string(CMD[1]);
display_string("\n");
} else {
display_string("set priority successful\n");
}
} else if (strcmp(CMD[0],"ps")==0){
if (show_tasks(CMD[1])==-1){
display_string("I can't recognize the parameter\n");
}
} else if (strcmp(CMD[0],"ls")==0){
ls();
} else if (strcmp(CMD[0],"rm")==0){
if (total<2){
display_string("Usage: rm <filename>\n");
} else{
if (rm(CMD[1])==-1){
display_string(CMD[1]);
display_string(" no exist!\n");
}
}
} else if (strcmp(CMD[0],"help")==0){
show_help();
} else if (strcmp(CMD[0],"edit")==0){
if (total==1){
display_string("Usage: edit <filename>\n");
} else start_editor(CMD[1]);
} else if (strcmp(CMD[0],"dump")==0){
if (dump_mem(CMD[1],CMD[2])==-1){
display_string("I can't recognize the parameter\n");
}
} else{
display_string(cmd); display_string(": ");
display_string("no such command\n");
}
}
int main() {
U32 T_LED = 0L; // time of last digit update
U32 Tow = 0L; // 1-wire time
U32 T_btns = 0L; // buttons timer
U8 old_btns_state = 0;
U8 show_temp = 0; // =0 to show number; = 1 to show temp
// Configure clocking
CLK_CKDIVR = 0; // F_HSI = 16MHz, f_CPU = 16MHz
// Configure pins
CFG_GCR |= 1; // disable SWIM
LED_init();
CCR |= 0x28; // make shure that we are on level 3 - disabled SW priority
// Configure Timer1
// prescaler = f_{in}/f_{tim1} - 1
// set Timer1 to 1MHz: 16/1 - 1 = 15
TIM1_PSCRH = 0;
TIM1_PSCRL = 15; // LSB should be written last as it updates prescaler
// auto-reload each 1ms: TIM_ARR = 1000 = 0x03E8
TIM1_ARRH = 0x03;
TIM1_ARRL = 0xE8;
// interrupts: update
TIM1_IER = TIM_IER_UIE;
// auto-reload + interrupt on overflow + enable
TIM1_CR1 = TIM_CR1_APRE | TIM_CR1_URS | TIM_CR1_CEN;
onewire_setup();
eeprom_default_setup();
// enable all interrupts
enableInterrupts();
set_display_buf("---");
if(get_starting_val()){
matchROM = 1; // if ROMs not empty, start scanning
starting_val = 0; // reset starting value
}
start_temp_reading();
// Loop
do {
if(((U16)(Global_time - T_time) > temper_delay) || (T_time > Global_time)){
T_time = Global_time;
if(show_temp){ // show temperature
temper_delay = TEMPER_delay;
show_temp = 0;
if(!temp_ready || temp_readed == ERR_TEMP_VAL){
set_display_buf("eab");
}else{
temp_ready = 0;
if(temp_readed > -100 && temp_readed < 1000){
display_int((int)temp_readed, 0);
display_DP_at_pos(1);
}else{ // display only integer part
display_int((int)temp_readed/10, 0);
}
}
if(matchROM) ++starting_val;
}else{ // show number
show_temp = 1;
temper_delay = NUMBER_delay;
if(matchROM){
if(get_starting_val()){
display_int(starting_val+1,0);
read_next_sensor();
}else{
starting_val = 0;
if(start_temp_reading()){
if(!get_starting_val()){
matchROM = 0;
set_display_buf("eee");
}else{
display_int(starting_val+1,0);
read_next_sensor();
}
}
}
}else{
start_temp_reading();
}
}
}
if((U8)(Global_time - T_LED) > LED_delay){
T_LED = Global_time;
show_next_digit();
}
if((U8)(Global_time - T_btns) > BTNS_delay){
T_btns = Global_time;
if(!block_keys && old_btns_state != buttons_state){
U8 pressed = old_btns_state & ~buttons_state; // pressed buttons are ones
if(pressed){ // some buttons were pressed
if(pressed & STORE_BTN){
if(onewire_reset()){
delete_notexistant = 0;
block_keys = 1;
onewire_send_byte(OW_READ_ROM);
while(OW_BUSY);
ow_process_resdata = store_last_ROM;
onewire_receive_bytes(8);
}
}else if(pressed & DELETE_BTN){
delete_notexistant = 1;
}else if(pressed & DELALL_BTN){
U8 i;
for(i = 0; i < MAX_SENSORS; i++){
erase_saved_ROM(i);
set_display_buf("---");
matchROM = 0;
}
}
//display_int(pressed, 0);
}else{ // some buttons released
//display_int(~old_btns_state & buttons_state, 0); // released are ones
}
}
old_btns_state = buttons_state;
}
if(Global_time != Tow){ // process every byte not frequently than once per 1ms
Tow = Global_time;
process_onewire();
}
if(waitforread){
if(onewire_reset()){
ow_process_resdata = send_read_seq;
waitforread = 0;
if(!matchROM){
ow_data_array[0] = OW_READ_SCRATCHPAD;
ow_data_array[1] = OW_SKIP_ROM;
onewire_send_bytes(2);
}else{
read_next_sensor();
}
}
}
} while(1);
}
int main() {
U32 T_LED = 0L; // time of last digit update
U32 T_time = 0L; // timer
int U = 0;
U8 vcap = 0; // ADC selection: 1 - vcap, 0 - ain
int pidx = 0; // index in median buffer
long voltagein = 0L, voltagecap = 0L; // variables for average values: vin and vcap
long cntrin = 0, cntrcap = 0; // counters for average
// Configure clocking
CLK_CKDIVR = 0; // F_HSI = 16MHz, f_CPU = 16MHz
// Configure pins
CFG_GCR |= 1; // disable SWIM
LED_init();
CCR |= 0x28; // make shure that we are on level 3 - disabled SW priority
// Configure Timer1
// prescaler = f_{in}/f_{tim1} - 1
// set Timer1 to 1MHz: 16/1 - 1 = 15
TIM1_PSCRH = 0;
TIM1_PSCRL = 15; // LSB should be written last as it updates prescaler
// auto-reload each 1ms: TIM_ARR = 1000 = 0x03E8
TIM1_ARRH = 0x03;
TIM1_ARRL = 0xE8;
// interrupts: update
TIM1_IER = TIM_IER_UIE;
// auto-reload + interrupt on overflow + enable
TIM1_CR1 = TIM_CR1_APRE | TIM_CR1_URS | TIM_CR1_CEN;
//eeprom_default_setup();
// configure ADC
// select PD3[AIN4] - AIN .. PD6[AIN6] - Vcap & enable interrupt for EOC
ADC_CSR = 0x24; // 0x24 - AIN4, 0x26 - AIN6
ADC_TDRL = 0x50; // disable Schmitt triger for AIN4 & AIN6
// right alignment
ADC_CR2 = 0x08; // don't forget: first read ADC_DRL!
// f_{ADC} = f/18 & continuous non-buffered conversion & wake it up
ADC_CR1 = 0x73; // 0x71 - single, 0x73 - continuous
ADC_CR1 = 0x73; // turn on ADC (this needs second write operation)
// enable all interrupts
enableInterrupts();
set_display_buf(" E0 "); // low power -> infinitive rebooting
// Loop
do {
// onse per 300ms refresh displayed value
if(((unsigned int)(Global_time - T_time) > 300) || (T_time > Global_time)){
T_time = Global_time;
voltagein /= cntrin; // average
voltagecap /= cntrcap;
// voltage = 1800 * ADUin / ADUcap
voltagein *= 1800;
U = (int)(voltagein/voltagecap);
display_int(U, 1);
cntrin = 0;
cntrcap = 0;
voltagein = 0L;
voltagecap = 0L;
pidx = 0;
}
if((U8)(Global_time - T_LED) > LED_delay){
if(ADC_ready){
// prepare data for rounded output
p[pidx] = ADC_value;
if(++pidx == 9){ // buffer is ready
if(vcap){
voltagecap += (long)(opt_med9());
cntrcap++;
ADC_CSR = 0x24; // switch to ain
vcap = 0;
}else{
voltagein += (long)(opt_med9());
cntrin++;
ADC_CSR = 0x26; // switch to vcap
vcap = 1;
}
pidx = 0;
}
ADC_ready = 0;
}
T_LED = Global_time;
show_next_digit();
}
} while(1);
}
void display_text_int(char *str,int number)
{
display_text(str);
display_int(number);
}
/**
* 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;
}
int main() {
unsigned long T_LED = 0L; // time of last digit update
unsigned long T_time = 0L; // timer
int i = 00, j = 00;
U8 beep_delay = 0;
int show_time_delay = 0;
U8 counter_enabled = 0;
key_state = 0;
work_hours = (work_hours_t*)EEPROM_START_ADDR;
keys_scan_buffer[0] = keys_scan_buffer[1] = keys_scan_buffer[2] = keys_scan_buffer[3] = 0;
Global_time = 0L; // global time in ms
ADC_value = 0; // value of last ADC measurement
LED_delay = 1; // one digit emitting time
// Configure clocking
CLK_CKDIVR = 0; // F_HSI = 16MHz, f_CPU = 16MHz
// Configure pins
CFG_GCR |= 1; // disable SWIM
LED_init();
// Configure Timer1
// prescaler = f_{in}/f_{tim1} - 1
// set Timer1 to 1MHz: 1/1 - 1 = 15
TIM1_PSCRH = 0;
TIM1_PSCRL = 15; // LSB should be written last as it updates prescaler
// auto-reload each 1ms: TIM_ARR = 1000 = 0x03E8
TIM1_ARRH = 0x03;
TIM1_ARRL = 0xE8;
// interrupts: update
TIM1_IER = TIM_IER_UIE;
// auto-reload + interrupt on overflow + enable
TIM1_CR1 = TIM_CR1_APRE | TIM_CR1_URS | TIM_CR1_CEN;
// configure ADC
BEEP_CSR = 0x1e; // Configure BEEP
_asm("rim"); // enable interrupts
display_int(i);
show_next_digit(); // show zero
// Loop
do
{
U8 *test = (U8*)0x4010;
U8 result;
if(((unsigned int)(Global_time - T_time) > DIGIT_PER) || (T_time > Global_time)) // set next timer value
{
T_time = Global_time;
if(i && counter_enabled == 2)
{
PA_ODR |= (1<<2); // Relay is on
i--;
if(!i)
{
counter_enabled = 0;
}
}
else
{
PA_ODR &= ~(1<<2); // Relay is off
}
if((i % 100) > 59)
{
i -= 40;
}
if(counter_enabled == 1)
{
if(j)
{
j--;
if(!j)
{
i++;
}
if((j % 100) > 59)
{
j -= 40;
}
}
if(!j)
{
counter_enabled = 2;
add_minutes_to_eeprom(i/100);
}
}
if(counter_enabled == 1)
{
display_int(-j);
}
else
{
display_int(i);
}
//if(i > 9999) i = -1200;
// check ADC value to light up DPs proportionaly
// values less than 206 == 0
}
if((U8)(Global_time - T_LED) > LED_delay)
{
T_LED = Global_time;
show_next_digit();
if(beep_delay)
{
beep_delay--;
if(!beep_delay)
{
BEEP_CSR = 0x1e; // Configure BEEP
}
}
if(show_time_delay)
{
show_time_delay--;
if(!show_time_delay)
{
i = 0; // Stop show time
}
}
}
result = scan_keys();
if(result & KEY_0_PRESSED) // Start
{
if(counter_enabled == 1 && j < 859) // 859 - wait 3 sec from 1-st start press
{
BEEP_CSR = 0xbe;
beep_delay = 200;
counter_enabled = 2;
add_minutes_to_eeprom(i/100);
i++;
j = 0;
}
if(!counter_enabled)
{
BEEP_CSR = 0xbe;
beep_delay = 10;
if(show_time_delay == 0 && i>0)
{
counter_enabled = 1;
j = 901; // 6 minutes pre time
}
else
{
// Show Time
i = work_hours->minutes + (int)(work_hours->hours_L) * 100 + (int)(work_hours->hours_H) * 10000;
show_time_delay = 2450;
}
}
}
else
{
if(result && show_time_delay)
{
i = 0;
show_time_delay = 0;
}
if(!counter_enabled)
{
if(result & KEY_3_PRESSED)
{
i+=100;
display_int(i);
BEEP_CSR = 0xbe;
beep_delay = 10;
}
if(result & KEY_2_PRESSED)
{
if(i >= 100)
{
i-=100;
display_int(i);
}
BEEP_CSR = 0xbe;
beep_delay = 10;
}
}
}
if(result & KEY_1_PRESSED) //Stop
{
counter_enabled = 0;
j = i = 0;
display_int(i);
BEEP_CSR = 0xbe;
beep_delay = 40;
show_time_delay = 0;
}
if((result & KEY_PRESSED) == KEY_PRESSED && Global_time < 1000)
{
BEEP_CSR = 0xbe;
beep_delay = 40;
clear_eeprom();
}
} while(1);
}
void main()
{
int i=10,j,k,N=24;
tInt bias,scale;
tLong frequency;
tChar temp;
// __bit test_resultA;
FLG_CPL=0;
lcd_init();
display("Initializing...");
enable_spi(0);
dac_set(0);
chip_init_CCFFE();
pll_init();
clearscr();
enable_serial();
display("Set Jumpers and ping");
temp = receive_serial();
clearscr();
display(" CCFFE");
line2();
display("Throughput test");
msDelay(1000);
enable_serial();
load_test_data();
clearscr();
display("Setting up test");
line2();
display("With 7 bit PRBS");
scale = 2650/N;
for(i=N;i>0;i--)
{
for(j=0;j<20;j++)
{
chip_init_CCFFE();
sel_source(RING);
bias=scale*i;
dac_set(bias);
msDelay(1000);
for(k=j;k>=0;k--)
{
tick_RX_RC_CKIN();
msDelay(1);
}
frequency = fmeasure();
clearscr();
display("f(");
display_int(i);
display(",");
display_int(j);
display(")=");
display_freq(frequency);
write_test_data_CCFFE();
LF_SELECT = 0;
msDelay(1);
LF_SELECT = 1;
read_results_CCFFE();
test_result = cross_correlation();//source_data);
if(test_result)
{
FLG_CPL=1;
send_byte(fm_byte[0]);
send_byte(fm_byte[1]);
send_byte(fm_byte[2]);
send_int(i);
send_byte(',');
send_int(j);
}
}
}
clearscr();
display("Throughput test");
line2();
if(FLG_CPL)
display("SUCCESS");
else
display("Failed");
while(1);
}
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;
}
