Path::Path(Point s, Point e):start(s),end(e),edge(start, end){ prev = next = NULL; vec = end - start; vec = vec.normalize(); to_next = get_orientation(vec); to_prev = get_orientation(start-end); set_angel(); }
void main (void) { int reflection,LDs_count,omron_count,angel; int old_fix,fix,left,right; unsigned char reflect,status,k; char old_flag,flag; WDTCN = 0xDE; // disable watchdog timer WDTCN = 0xAD; SYSCLK_Init (); // Initialize system clock to 16MHz ExtCrystalOsc_Init (); // switch system clock Port_Init (); // Initialize crossbar and GPIO SPI0_Init(); LED_Init(); // Initialize OLED Sensors_Init(); Rudder_Init(); Motor_Init(); Measurement_Init(); Test_Helper_Init(); EA = 1; // Enable global interrupts CR = 1; // start to output PWM P31 = 1; PUT_LINE("OMRON:",omron_count); PUT_LINE("reflection:",reflection); PUT_LINE("LDs_count:",LDs_count); PUT_LINE("Tunnel1:",tunnel_length[0]); PUT_LINE("Tunnel2:",0); PUT_LINE("Tunnel3:",0); //PUT_LINE("angel:",angel); //PUT_LINE("fix:",fix); while (1) { // Spin forever DIRECTION = 0; PCA0CPH1 = 0xE0; flag = 0; while (1) { // dectecting reflect = get_LD_reflection(); old_flag = flag; flag = 0; // conner emergency if(reflect < 25){ if(!LEFT3 && RIGHT3) flag = 1; if(!RIGHT3 && LEFT3) flag = -1; if(flag) break; } // go straight forward: if(!LEFT1 || !LEFT2 || !LEFT3) flag = 1; if(!RIGHT1 && LEFT2) flag = -1; if(!RIGHT2 && LEFT3) flag = -1; if(!RIGHT3) flag = -1; k = 7; if(reflect > 50){ k = 10; } if(reflect > 85){ if(flag > 0) if(++left > 32766) { left = left - right; right = 0; } if(flag < 0) if(++right > 32766) { right = right - left; left = 0; } old_fix = fix; fix = left - right; if(fix > 20) fix = 20; if(fix < -20) fix = -20; fix = ((fix + RUDDER_FIX) + old_fix)/2; } //UPDATE_VALUE(5,fix); if(MAX_REFLECTION > reflect) angel = MAX_REFLECTION - reflect; else angel = 0; angel = flag * k * angel / 10 + fix; //UPDATE_VALUE(4,angel); set_angel(angel); // debug info omron_count = Get_OMRON_Count(); reflection = get_LD_reflection(); LDs_count = TL1 + LD2_count; UPDATE_VALUE(0,omron_count); UPDATE_VALUE(1,reflection); UPDATE_VALUE(2,LDs_count/3); tmp = tunnel_length[1] * 464 / 1683; UPDATE_VALUE(3,tmp); tmp = tunnel_length[2] * 464 / 1683; UPDATE_VALUE(4,tmp); tmp = tunnel_length[3] * 464 / 1683; UPDATE_VALUE(5,tmp); } reflect = get_LD_reflection(); if(reflect < 10){ // 60 degree PCA0CPH1 = 0xB8; set_angel(flag * 127); Clear_OMRON_Count(); while(omron_count <= 300); while(1){ if(flag > 0 && !LEFT1) { while(!RIGHT1); break; } if(flag < 0 && !RIGHT1) { while(!LEFT1); break; } } }else{ // 90 degree DIRECTION = 1; PCA0CPH1 = 0x38; Clear_OMRON_Count(); set_angel(-flag * 127); while(1){ omron_count = Get_OMRON_Count(); UPDATE_VALUE(0,omron_count); if(omron_count >= 410) //1062 break; } DIRECTION = 0; PCA0CPH1 = 0xB8; //Clear_OMRON_Count(); set_angel(flag * 127); // escape from conner while(1){ if(flag > 0 && !LEFT1){ while(!RIGHT1); break; } if(flag < 0 && !RIGHT1){ while(!LEFT1); break; } // if } // while } // if else: 60 or 90 degree } // main loop } // main