/**
* 反転中
*/
static void AC_reversal(int state) {
DU_showString("5thPhase");
MU_backward();
systick_wait_ms(1500);
LDC_pre();
LDC_quickLeft();
systick_wait_ms(2500);
while(!LMU_isOnline()){
LDC_reverseLeft();
}
AC_changeStatus(AC_RETURN);
}
void CA_set_threshold(void) {
/* LineMonitorUnitにセンサの値をセットする */
while(FALSE == BU_is_touched()){}
LMU_set_black_value(LMU_is_line_value());
HU_ok_sound();
systick_wait_ms(1000);
while(FALSE == BU_is_touched()){}
LMU_set_white_value(LMU_is_line_value());
HU_ok_sound();
systick_wait_ms(1000);
/*while(FALSE == BU_is_touched()){}
LMU_set_gray_value(LMU_is_line_value());
HU_ok_sound();
systick_wait_ms(1000);*/
while(FALSE == BU_is_touched()){}
LMU_set_threshold(LMU_is_line_value());
HU_ok_sound();
systick_wait_ms(1000);
while(FALSE == BU_is_touched()){}
systick_wait_ms(500);
/* BalancerUnitのオフセット値をセットする */
BLNU_set_gyro_offset();
}
//キャリブレーション関数
void RN_calibrate()
{
/*バランサーON用*/
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(932, 512, 10);
gyro_offset += (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1);
systick_wait_ms(500);
break;
}
}
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(982,512,10);
setting_mode = RN_RUN;
runner_mode = RN_MODE_BALANCE;
systick_wait_ms(500);
break;
}
}
}
void crap_send()
{
systick_wait_ms(50);
U8 crap[5] = {0,0,0,0,0};
ecrobot_send_rs485(crap, 0, 5);
systick_wait_ms(50);
}
int Calibration_calibration(Calibration *this_Calibration){
static U32 avg_cnt = 0;
static U32 cal_start_time;
//gyro_offset
while (1){
if(PushButton_detect_push_button(&pushButton) == TRUE)break;
}
ecrobot_sound_tone(880, 512, 10);
/* ジャイロセンサの値を計算するための開始時間をセットする */
cal_start_time = ecrobot_get_systick_ms();
while((ecrobot_get_systick_ms() - cal_start_time) < 1000U){
/* ジャイロセンサの設定をする */
this_Calibration->gyro += InclinationEncoder_get_inclination(&inclinationEncoder);
avg_cnt++;
}
this_Calibration->gyro /= avg_cnt;
ecrobot_sound_tone(440U, 500U, 10);
systick_wait_ms(500);
//black
while(1){
if(PushButton_detect_push_button(&pushButton) == TRUE){
ecrobot_sound_tone(906, 512, 10);
BrightnessEncoder_set_black(&brightnessEncoder,BrightnessEncoder_get_brightness(&brightnessEncoder));
systick_wait_ms(500);
break;
}
}
//white
while(1){
if(PushButton_detect_push_button(&pushButton) == TRUE){
ecrobot_sound_tone(906, 512, 10);
BrightnessEncoder_set_white(&brightnessEncoder,BrightnessEncoder_get_brightness(&brightnessEncoder));
systick_wait_ms(500);
break;
}
}
InclinationEncoder_set_gyro_offset(&inclinationEncoder ,this_Calibration->gyro);
InclinationEncoder_set_initial_gyro_offset(&inclinationEncoder ,this_Calibration->gyro);
while(1){
if(PushButton_detect_push_button(&pushButton) == TRUE){
systick_wait_ms(500);
break;
}
}
//キャリブレーションが終了したら1を返す
return 1;
}
// Esta funcion toma tres muestras del sonar y se queda con la media
int obtener_distancia()
{
int sensor1, sensor2, sensor3, sensor;
sensor1 = ecrobot_get_sonar_sensor(SONAR_PORT);
systick_wait_ms(20);
sensor2 = ecrobot_get_sonar_sensor(SONAR_PORT);
systick_wait_ms(20);
sensor3 = ecrobot_get_sonar_sensor(SONAR_PORT);
systick_wait_ms(20);
sensor = (sensor1+sensor2+sensor3)/3;
// if(sensor == 255) sensor = 0;
return (sensor);
}
/**
* Write a page from the supplied flash buffer to flash memory
* The flash buffer must have been obtained through a call to
* flash_get_page_buffer, before making this call
* returns > 0 number of bytes written < 0 error
* Error returns:
* -1 Timeout waiting for TWI to complete
* -2 Timeout waiting for flash write to complete
* -3 Bad page number
* -4 bad flash buffer
*/
int
flash_write_page_buffer(FOURBYTES *page, int page_num)
{
/* Write page to flash memory.
* This function must run out of ram and while it executes no other code
* (especially any flash resident) code must run. This is becuase the
* flash memory is only a single plane and can not be accessed for both read
* and write at the same time.
*/
int istate;
int status;
if (page_num + flash_start_page >= FLASH_MAX_PAGES) return -3;
if (VINTPTR(page) != &(FLASH_BASE[page_num*FLASH_PAGE_SIZE])) return -4;
/* We must disbale interrupts. However we need to try and ensure that all
* current interrupt activity is complete before we do that. We talk to
* the avr every 1ms and this uses interrupt driven I/O so we try to make
* sure this is complete.
*/
// Allow any playing sound to complete
sound_wait();
// Turn off timer tick call backs
systick_suspend();
// Wait until next tick
systick_wait_ms(1);
// Force a tick to talk to the avr
nxt_avr_1kHz_update();
// Wait for it to complete
status = wait_twi_complete();
if (status != 0) return -1;
// Now we can turn off all ints
istate = interrupts_get_and_disable();
// Write the buffer to the selected page
status = flash_write(page_num + flash_start_page);
// Turn ints back on
if (istate) interrupts_enable();
// Ensure that we are back in-sync.
systick_wait_ms(1);
// Allow call backs on 1ms tick
systick_resume();
if (!(status & AT91C_MC_FRDY)) return -2;
return FLASH_PAGE_SIZE*sizeof(U32);
}
//*****************************************************************************
// TASK : tsk2
// DESCRIPTION : Background(never terminated) Task
//*****************************************************************************
void tsk2(VP_INT exinf)
{
while(1){
ecrobot_status_monitor("NXTway-GS JSP"); /* LCD display */
systick_wait_ms(500U); /* 500msec wait */
}
}
void sound_wait()
{
// Allow any currently playing sample to complete.
int i = sound_get_time();
if (i > 0)
systick_wait_ms(i + 2);
}
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;
}
// キャリブレーションを行う
U16 UI::calibrate(float angle){
// タッチセンサの入力を待つ
while(1){
tail->control(angle);
if(touchSensor->isPressed()){
break;
}
// 10msecウェイトする
systick_wait_ms(10);
}
// 1000msecウェイトする
systick_wait_ms(1000);
return lightSensor->getBrightness();
}
void tpl_shutdown(void)
{
/* Enable interrupts because disabled bu ShutdownOS and we
need them to shutdown the NXT */
ENABLE_FIQ();
ENABLE_IRQ();
ecrobot_term_bt_connection(); /* shutdown bluetooth connection */
display_clear(1);
systick_wait_ms(10);
nxt_lcd_power_down(); /* reset LCD hardware */
systick_wait_ms(10);
while(1)
{
nxt_avr_power_down();
}
}
void do_tyreal(float *Kp_t,float *Ki_t,float *Kd_t){
//�ó‘Ô‘JˆÚ
if (ecrobot_is_RUN_button_pressed() == 1) {
ecrobot_sound_tone(932, 512, VOL);
systick_wait_ms(100);
ecrobot_sound_tone(466, 256, VOL);
systick_wait_ms(10);
systick_wait_ms(10);
if(adjust_param_state==ADJUST_Kp_VAL){
adjust_param_state=ADJUST_Ki_VAL;
}
else if(adjust_param_state==ADJUST_Ki_VAL){
adjust_param_state=ADJUST_Kd_VAL;
}
else if(adjust_param_state==ADJUST_Kd_VAL){
adjust_param_state=ADJUST_Kp_VAL;
}
}
switch(adjust_param_state) {
case (ADJUST_Kp_VAL): *Kp_t=change_float_param(*Kp_t);
break;
case (ADJUST_Ki_VAL): *Ki_t=change_float_param(*Ki_t);
break;
case (ADJUST_Kd_VAL): *Kd_t=change_float_param(*Kd_t);
break;
default:
break;
}
display_clear(1);
float_to_string(*Kp_t,float_string);
display_show_string("Kp",0,0);
display_show_string(float_string,0,1);
float_to_string(*Ki_t,float_string);
display_show_string("Ki",0,2);
display_show_string(float_string,0,3);
float_to_string(*Kd_t,float_string);
display_show_string("Kd",0,4);
display_show_string(float_string,0,5);
}
void tsk0(VP_INT exinf)
{
while(1)
{
ecrobot_status_monitor("JSP HelloWorld!");
systick_wait_ms(500); /* 500msec wait */
}
}
void bt_reset(void)
{
// Perform hardware reset. This function has some relatively long
// delays in it and so should probably only be called during application
// initialisation and termination. Calling it at other times may cause
// problems for other real time tasks.
// If power is currently off to the BC4 do not reset it!
if ((*AT91C_PIOA_ODSR & BT_RST_PIN) == 0) return;
//display_goto_xy(0, 1);
//display_string("BT Reset....");
//display_update();
//systick_wait_ms(10000);
// Ask for command mode
bt_clear_arm7_cmd();
// BC4 reset sequence. First take the reset line low for 100ms
bt_set_reset_low();
// Wait and discard any packets that may be around
int cnt = 100;
U8 *buf = out_buf[0];
while (cnt-- > 0)
{
bt_receive(buf);
systick_wait_ms(1);
}
bt_set_reset_high();
// Now wait either for 5000ms or for the BC4 chip to signal reset
// complete. Note we use the out buffer as a scratch area here
// this id safe since we are forcing a reset.
cnt = 5000;
while (cnt-- > 0)
{
bt_receive(buf);
// Look for the reset indication and the checksum
if ((buf[0] == 3) && (buf[1] == MSG_RESET_INDICATION) &&
(buf[2] == 0xff) && (buf[3] == 0xe9))
break;
systick_wait_ms(1);
}
// Force command mode
bt_clear_arm7_cmd();
//display_goto_xy(10, 1);
//display_int(cnt, 5);
//display_update();
//systick_wait_ms(10000);
}
void
systick_test(void)
{
while (1) {
test_counter++;
systick_wait_ms(2000);
}
}
void emergency_stop(char* errMsgLine1, char* errMsgLine2) {
motor_set_speed(NXT_PORT_A,0,1);
motor_set_speed(NXT_PORT_B,0,1);
motor_set_speed(NXT_PORT_C,0,1);
print_clear_display();
print_str(0,1,errMsgLine1);
print_str(0,2,errMsgLine2);
int i;
for(i = 0; i < 3; i++) {
print_str(6,4,"ERROR");
print_update();
ecrobot_set_light_sensor_active(NXT_PORT_S1);
ecrobot_set_light_sensor_active(NXT_PORT_S2);
ecrobot_set_light_sensor_active(NXT_PORT_S3);
ecrobot_sound_tone(1000,500,15);
systick_wait_ms(750);
print_clear_line(4);
print_update();
ecrobot_set_light_sensor_inactive(NXT_PORT_S1);
ecrobot_set_light_sensor_inactive(NXT_PORT_S2);
ecrobot_set_light_sensor_inactive(NXT_PORT_S3);
ecrobot_sound_tone(500,500,15);
systick_wait_ms(750);
}
motor_set_speed(NXT_PORT_A,0,0);
motor_set_speed(NXT_PORT_B,0,0);
motor_set_speed(NXT_PORT_C,0,0);
while(true) {
print_str(6,4,"ERROR");
print_update();
ecrobot_set_light_sensor_active(NXT_PORT_S1);
ecrobot_set_light_sensor_active(NXT_PORT_S2);
ecrobot_set_light_sensor_active(NXT_PORT_S3);
systick_wait_ms(750);
print_clear_line(4);
print_update();
ecrobot_set_light_sensor_inactive(NXT_PORT_S1);
ecrobot_set_light_sensor_inactive(NXT_PORT_S2);
ecrobot_set_light_sensor_inactive(NXT_PORT_S3);
systick_wait_ms(750);
}
}
int tyreal_trigger(){
int result=0;
if (ecrobot_is_RUN_button_pressed() == 1) {
systick_wait_ms(100);
result = 1;
}
return result;
}
// Function to decode a LIST of FISTS and do things accordingly... :3
void decode_bro_fists (const bro_fist_t * orders, bro_fist_t * response, engines_t * motors)
{
int i;
for (i = 0; i < BUFFER_SIZE; i ++) {
decode_bro_input (&orders[i], &response[i], motors);
systick_wait_ms(2);
};
};
//*****************************************************************************
// 関数名 : 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);
}
int
flash_write_page(int start_page, U32 *page, int page_num)
{
int i, istate;
int status;
if (page_num + start_page > 1023) return 0;
systick_suspend();
systick_wait_ms(1);
nxt_avr_1kHz_update();
// modified based on the latest leJOS source (April 16th takashic)
// Wait for it to complete
status = wait_twi_complete();
if (status != 0) return -1;
// Now we can turn off all ints
istate = interrupts_get_and_disable();
for (i = 0; i < 64; i++)
FLASH_BASE[(page_num*64)+i] = page[i];
FLASH_CMD_REG = (0x5A000001 + (((page_num + start_page) & 0x000003FF) << 8));
status = wait_flash_ready();
// modified based on the latest leJOS source (April 16th takashic)
// Turn ints back on
if (istate) interrupts_enable();
// Ensure that we are back in-sync.
systick_wait_ms(1);
// Allow call backs on 1ms tick
systick_resume();
if (!(status & AT91C_MC_FRDY)) return -1;
return 1;
}
void crap_recieve()
{
U8 crap_buf[5] = {0,0,0,0,0};
int crapBytes = 0;
crapBytes = 0;
while (crapBytes != 5)
{
crapBytes += ecrobot_read_rs485(crap_buf, crapBytes, 5 - crapBytes);
}
ecrobot_read_rs485(crap_buf, crapBytes, 1024);
systick_wait_ms(5);
ecrobot_read_rs485(crap_buf, crapBytes, 1024);
}
float change_float_param(float param){
static int temp =0;
if((int)nxt_motor_get_count(NXT_PORT_C) - temp>0){
param=param+ADJUST_FLOAT_STEP;
}
else if((int)nxt_motor_get_count(NXT_PORT_C) - temp<0){
param=param-ADJUST_FLOAT_STEP;
}
temp=(int)nxt_motor_get_count(NXT_PORT_C);
systick_wait_ms(100);
param= round_n(param,2);
return param;
}
void sonar_sensor_measurement(void)
{
// Variables used for distance measurement
S32 distance_left = 0;
S32 distance_right = 0;
int measurements = 0;
// Display menu
reset_sonar_measurement_menu();
while(true)
{
// Update values
distance_left += ecrobot_get_sonar_sensor(SONAR_SENSOR_FRONT);
distance_right += ecrobot_get_sonar_sensor(SONAR_SENSOR_REAR);
// Calculate average of new and previous
distance_left /= 2;
distance_right /= 2;
// Wait when over some measurements
if(measurements++ % MEASUREMENT_WAIT_INTERVAL == 0)
{
// Update the display
display_sonar_sensor_measurements(distance_left, distance_right);
systick_wait_ms(MEASUREMENT_WAIT);
}
// Chek if the enter button is pressed for long enough time to exit
if(ecrobot_is_ENTER_button_pressed())
{
// Time when the button was pressed
int enter_button_pressed_start_time = systick_get_ms();
// Continusly check if enought time has past to exit
while(ecrobot_is_ENTER_button_pressed())
{
if(enter_button_pressed_start_time + ENTER_BUTTON_EXIT_TIMEOUT <
systick_get_ms())
{
return;
}
}
}
}
}
int change_int_param(int param){
static int temp =0;
//�O��擾������]�p�Ƃ̍���p���Ēl�𐧌�
if((int)nxt_motor_get_count(NXT_PORT_C) - temp>0){
param=param+ADJUST_INT_STEP;
}
else if((int)nxt_motor_get_count(NXT_PORT_C) - temp<0){
param=param-ADJUST_INT_STEP;
}
temp=(int)nxt_motor_get_count(NXT_PORT_C);
systick_wait_ms(SET_PARAM_SPEED); /* 100msec�E�F�C�g */
return param;
}
float change_float_param(float param){
static int temp =0;
//�O��擾������]�p�Ƃ̍���p���Ēl�𐧌�
if((int)nxt_motor_get_count(NXT_PORT_C) - temp>0){
param=param+ADJUST_FLOAT_STEP/*0.001*/;
// param=(param+0.1)/*(float)ADJUST_FLOAT_STEP*/;
}
else if((int)nxt_motor_get_count(NXT_PORT_C) - temp<0){
param=param-ADJUST_FLOAT_STEP/*0.001*/;
//param=param-0.1;/*(float)ADJUST_FLOAT_STEP);*/
}
temp=(int)nxt_motor_get_count(NXT_PORT_C);
systick_wait_ms(SET_PARAM_SPEED); // 100msec�E�F�C�g
return param;
}
void
force_reset()
{
// reset the UDP connection.
int i_state;
// Take the hardware off line
*AT91C_PIOA_PER = (1 << 16);
*AT91C_PIOA_OER = (1 << 16);
*AT91C_PIOA_SODR = (1 << 16);
*AT91C_PMC_SCDR = AT91C_PMC_UDP;
*AT91C_PMC_PCDR = (1 << AT91C_ID_UDP);
systick_wait_ms(2);
// now bring it back online
i_state = interrupts_get_and_disable();
/* Make sure the USB PLL and clock are set up */
*AT91C_CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
*AT91C_PMC_SCER = AT91C_PMC_UDP;
*AT91C_PMC_PCER = (1 << AT91C_ID_UDP);
*AT91C_UDP_FADDR = 0;
*AT91C_UDP_GLBSTATE = 0;
/* Enable the UDP pull up by outputting a zero on PA.16 */
*AT91C_PIOA_PER = (1 << 16);
*AT91C_PIOA_OER = (1 << 16);
*AT91C_PIOA_CODR = (1 << 16);
*AT91C_UDP_IDR = ~0;
/* Set up default state */
reset();
*AT91C_UDP_IER = (AT91C_UDP_EPINT0 | AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM);
if (i_state)
interrupts_enable();
}
void getPathComplicated(signed char x1, signed char y1, signed char xDestination, signed char yDestination)
{
if((x1==xDestination)&&(y1==yDestination))
{
stopBoth();
ecrobot_sound_tone(100,500,100);
systick_wait_ms(2000);
endAll();
addToPath(none);
return;
}
//display_currentNode();
//systick_wait_ms(1000);
/*
display_clear(1);
display_goto_xy(0,0);
display_string("gPC called");
display_goto_xy(0,1);
display_int((int)x1,1);
display_goto_xy(3,1);
display_int((int)y1,1);
display_goto_xy(0,2);
display_int((int)xDestination,1);
display_goto_xy(3,2);
display_int((int)yDestination,1);
display_update();
*/
//systick_wait_ms(1000);
signed char xCurrent;
signed char yCurrent;
signed char startReached = 0;
signed char endReached = 0;
signed char xDistance= xDestination - x1;
signed char yDistance= yDestination - y1;
signed char yD2=0;
signed char xD2=0;
if(yDistance<0){yD2=-yDistance;}else{yD2=yDistance;}
if(xDistance<0){xD2=-xDistance;}else{xD2=xDistance;}
char maxIterations = (xD2+yD2)/2+40;//40 is safety padding
char cntLoops;
if(maxIterations<0)
maxIterations=-maxIterations;
//************************************** MAIN BODY ******
int x,y;
for(x=0;x<=12;x++)
{
for(y=0;y<=12;y++)
{
mazeStart[x][y]=0;
mazeEnd[x][y]=0;
}
}
mazeStart[x1][y1]=50;
int colX=0;
int rowY=0;
mazeEnd[xDestination][yDestination]=50;
signed char l,r,t,d;
for(cntLoops=0; cntLoops<=maxIterations*2; cntLoops++)
{
if((startReached==1) && (endReached==1))
{
cntLoops = (maxIterations*2)+1;
}
//***************************set mazeStart's values checks now if values overflow
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
if(mazeStart[xDestination][yDestination]!=0){endReached = 1;break;};
//check if the field has the path to the adjecant field
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeStart[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeStart[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeStart[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeStart[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeStart[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeStart[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeStart[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeStart[colX][rowY-1]-1;
}
mazeStart[colX][rowY]=getLargestValue(l,t,r,d,mazeStart[colX][rowY]);
}
}
//*************************** set mazeEnd's values
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
if((mazeEnd[x1][y1]!=0)){startReached = 1;break;};
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeEnd[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeEnd[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeEnd[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeEnd[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeEnd[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeEnd[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeEnd[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeEnd[colX][rowY-1]-1;
}
mazeEnd[colX][rowY]=getLargestValue(l,t,r,d,mazeEnd[colX][rowY]);
}
}
}
//prnt values both matrixes now added into mazeStart
int ty, tx;
tx=ty=0;
for(ty=12; ty>=0; ty--)
{
for(tx=0; tx<=12; tx++)
{
mazeStart[tx][ty]=mazeStart[tx][ty]+mazeEnd[tx][ty];
mazeEnd[tx][ty]=0;
}
}
//now add way of highest numbers to path so it can be printed or driven
xCurrent = x1;
yCurrent = y1;
signed char xDisLeft = xDistance;
signed char yDisLeft = yDistance;
signed char pathValue = mazeStart[x1][y1];
mazeEnd[xCurrent][yCurrent]=1; //repurposing mazeE to store already visited Nodes
if(endReached==1)
{
while((xCurrent!=xDestination) || (yCurrent != yDestination))
{
if((xDisLeft>0)||(((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue||(hasDirection(left,xCurrent,yCurrent)==0)))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))))
{
if(hasDirection(right,xCurrent,yCurrent) && (mazeStart[xCurrent+1][yCurrent]==pathValue) && (mazeEnd[xCurrent+1][yCurrent]==0))
{
addToPath(right);
mazeEnd[xCurrent][yCurrent]=1;
xCurrent++;
xDisLeft--;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(xDisLeft<0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))))
{
if(hasDirection(left,xCurrent,yCurrent) && (mazeStart[xCurrent-1][yCurrent]==pathValue) && (mazeEnd[xCurrent-1][yCurrent]==0))
{
addToPath(left);
mazeEnd[xCurrent][yCurrent]=1;
xCurrent--;
xDisLeft++;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(yDisLeft>0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue)||(hasDirection(left,xCurrent,yCurrent)==0))))
{
if(hasDirection(top,xCurrent,yCurrent) && (mazeStart[xCurrent][yCurrent+1]==pathValue) && (mazeEnd[xCurrent][yCurrent+1]==0))
{
addToPath(top);
mazeEnd[xCurrent][yCurrent]=1;
yCurrent++;
yDisLeft--;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(yDisLeft<0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue)||(hasDirection(left,xCurrent,yCurrent)==0))))
{
if(hasDirection(down,xCurrent,yCurrent) && (mazeStart[xCurrent][yCurrent-1]==pathValue) && (mazeEnd[xCurrent][yCurrent-1]==0))
{
addToPath(down);
mazeEnd[xCurrent][yCurrent]=1;
yCurrent--;
yDisLeft++;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
}
}
/*
display_goto_xy(0,3);
display_string("ret:");
display_goto_xy(4,3);
display_int(pathStorage[1],1);
display_update();*/
//systick_wait_ms(2000);
}
void getNextUnvisited()
{
checkIfAllVisited();
signed char x1 = currentPosition[0];
signed char y1 = currentPosition[1];
signed char cnt =0;
signed char colX=0;
signed char compare;
signed char rowY=0;
signed char foundOne=0;
//ecrobot_sound_tone(900,90,100);
if((allVisited)&&(getTokensFound()!=3))
{
addToPath(none);
return;
}
if(getTokensFound()==3)
{
getPathComplicated(x1,y1,6,6);
return;
}
signed char l,t,r,d;
for(colX=0; colX<=12;colX++){
for(rowY=0; rowY<=12;rowY++){
mazeVisited[colX][rowY]=visited(colX,rowY);
mazeDistance[colX][rowY]=0;
}
}
//now start calculatin'
mazeDistance[x1][y1]=50;
for(cnt=0; cnt<=35 ; cnt++)//nah, don't calculate how much: just do it ;)
{
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
//if((mazeDistance[x1][y1]!=0)){foundOne !=0 ;break;};
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeDistance[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeDistance[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeDistance[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeDistance[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeDistance[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeDistance[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeDistance[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeDistance[colX][rowY-1]-1;
}
mazeDistance[colX][rowY]=getLargestValue(l,t,r,d,mazeDistance[colX][rowY]);
}
}
}
for(colX=0; colX<=12;colX++){
for(rowY=0; rowY<=12;rowY++){
mazeDistance[colX][rowY]=50-mazeDistance[colX][rowY];
}
}
for(compare=0; compare<=35;compare++)
{
if(foundOne){return;};
for(rowY=12;rowY>=0;rowY--)
{
if(foundOne){return;};
for(colX=0;colX<=12;colX++)
{
if((mazeVisited[colX][rowY]==0)&&(mazeDistance[colX][rowY]==compare))
{
getPathComplicated(x1,y1,colX,rowY);
foundOne =1;
return;
}
}
}
}
//alle besucht
ecrobot_sound_tone(400,250,100);
systick_wait_ms(250);
ecrobot_sound_tone(900,250,100);
systick_wait_ms(250);
if(!allVisited)
getPathComplicated(x1,y1,6,6);
setAllVisited(1);
}
//キャリブレーション関数
void RN_calibrate(){
tail_mode_change(0,ANGLEOFDOWN,1,2);
//黒値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE){
ecrobot_sound_tone(880, 512, 10);
BLACK_VALUE=ecrobot_get_light_sensor(NXT_PORT_S3);
systick_wait_ms(500);
break;
}
}
//白値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE){
ecrobot_sound_tone(906, 512, 10);
WHITE_VALUE=ecrobot_get_light_sensor(NXT_PORT_S3);
systick_wait_ms(500);
break;
}
}
//灰色値計算
GRAY_VALUE=(BLACK_VALUE+WHITE_VALUE)/2;
//ジャイロオフセット及びバッテリ電圧値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE){
ecrobot_sound_tone(932, 512, 10);
gyro_offset += (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1);
systick_wait_ms(500);
break;
}
}
//走行開始合図
while(1){
//リモートスタート
if(remote_start()==1){
ecrobot_sound_tone(982,512,30);
tail_mode_change(1,ANGLEOFUP,0,2);
setting_mode = RN_SPEEDZERO;
runner_mode = RN_MODE_BALANCE;
break;
}
//タッチセンサスタート
else if (ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE){
ecrobot_sound_tone(982,512,10);
while(1){
if (ecrobot_get_touch_sensor(NXT_PORT_S4) != TRUE){
setting_mode = RN_SPEEDZERO;
runner_mode_change(1);
tail_mode_change(1,ANGLEOFUP,0,2);
break;
}
}
break;
}
}
}