void main(void) { vInitialize(); myInit(); while(!pushsw_get()) { if(cnt1<100) led_out(0x1); else if(cnt1<200) led_out(0x2); else cnt1 = 0; } cnt1 = 0; while(startbar_get()) { if(cnt1<50) led_out(0x1); else if(cnt1<100) led_out(0x2); else cnt1 = 0; } led_out(0); start(); pattern = 10; while(1) { switch(pattern) { case 10: //直线 if(check_crossline()){ pattern = 30;break; } if(check_blackArea()){ pattern = 40;break; } straight_run(); //直线前行,弯道检测,盲区检测 break; case 20: //弯道 switch(turn_direction) { case LEFT: handle(-1*midDegree_st4); left_turn(); break; case RIGHT: handle(midDegree_st4); right_turn(); break; default:break; } break; case 30: //直角 rightAngle(); break; case 40: //盲区 switch(blackArea_direction) { case LEFT: left_blackArea(); break; case RIGHT: right_blackArea(); break; default:break; } break; default:break; }//end switch }//end while }
int main() { init_MCU(); sel_mode(); while (1) { switch (pattern) { case 1: if ( check_crossline() ) //Be 90 { pattern = 21; break; } if ( check_rightline() ) // Chuyen lan phai { pattern = 51; break; } if ( check_leftline() ) // Chuyen lan trai { pattern = 61; break; } led7(10); set_encoder(19); switch (doc_cam_bien(0b01111110)) { case 0b01111110: toc_do(0, 0); goc(0); break; case 0b00011000: // Chay thang goc( 0 ); toc_do( 100 , 100 ); break; //lech phai case 0b00011100: case 0b00001000: toc_do(90, 80); goc(8); break; case 0b00001100: toc_do(90, 70); goc(10); break; case 0b00001110: case 0b00000100: toc_do(90, 60); goc(12); break; case 0b00000110: toc_do(90, 50); goc(15); break; case 0b00000010: toc_do(90, 30); goc(20); pattern = 11; //Lệch phải góc lớn break; ////////////////////////////////////////// //lech trai case 0b00111000: case 0b00010000: toc_do(80, 90); goc(-8); break; case 0b00110000: toc_do(70, 90); goc(-10); break; case 0b01110000: case 0b00100000: toc_do(60, 90); goc(-12); break; case 0b01100000: toc_do(50, 90); goc(-15); break; case 0b01000000: toc_do(30, 90); goc(-20); pattern = 12; //Lệch trái góc lớn break; default: break; } break; case 11: //Lệch phải góc lớn led7(11); set_encoder(23); switch (doc_cam_bien(0b11001100)) { case 0b11000000: toc_do(95, 15); goc(75); break; case 0b10000000: toc_do(95, 20); goc(51); break; case 0b00000000: toc_do(95, 25); goc(48); break; case 0b00000100: toc_do(95, 25); goc(37); break; case 0b00001100: toc_do(95, 25); goc(29); pattern = 1; break; default: pattern = 11; } break; case 12: //Lệch trái góc lớn led7(12); set_encoder(23); switch (doc_cam_bien(0b00110011)) { case 0b00000011: toc_do(15, 95); goc(-73); break; case 0b00000001: toc_do(20, 95); goc(-50); break; case 0b00000000: toc_do(25, 95); goc(-47); break; case 0b00100000: toc_do(25, 95); goc(-36); break; case 0b00110000: toc_do(25, 95); goc(-28); pattern = 1; break; default: pattern = 12; } break; case 21: //Xử lý khi gặp vạch tín hiệu cua vuông đầu tiên led7(18); set_encoder(20); goc( 0 ); toc_do( 60 , 60); pattern = 22; cnt_1ms = 0; xung = 0; break; case 22: // Delay hết vạch led7(22); set_encoder(20); goc( 0 ); toc_do( 60 , 60); if ( cnt_1ms > 150 || xung > 10 ) { pattern = 23; cnt_1ms = 0; xung = 0; } break; case 23: led7(23); //cua trái if ( (xung > 100) && ((doc_cam_bien(0b11111111) == 0b11111000) || (doc_cam_bien(0b11111111) == 0b11110000) || (doc_cam_bien(0b11111111) == 0b11100000) || (doc_cam_bien(0b11111111) == 0b11111100))) // Neu gap tin hieu nay la goc cua 90 trai thi be { pattern = 24; xung = 0; cnt_1ms = 0; break; } //cua phải if ( (xung > 100) && ((doc_cam_bien(0b11111111) == 0b00011111 ) || (doc_cam_bien(0b11111111) == 0b00000111) || (doc_cam_bien(0b11111111) == 0b00001111) || (doc_cam_bien(0b11111111) == 0b00111111))) // Neu gap tin hieu nay la goc cua 90 phai thi be { pattern = 25; xung = 0; cnt_1ms = 0; break; } if ( (xung > 30) && (xung < 100) && check_crossline() == 1) { pattern = 71; xung = 0; cnt_1ms = 0; break; } // Nguoc lai thi chinh thang cho xe switch (doc_cam_bien(0b01111110)) { case 0b00011000: goc( 0 ); toc_do(30, 30 ); break; //lech phai case 0b00001000: toc_do(23, 3); goc(8); break; case 0b00001100: toc_do(23, -3); goc(10); break; case 0b00000100: toc_do(23, -8); goc(13); break; case 0b00000110: case 0b00000010: toc_do(23, -13); goc(18); break; //////////////////////////////// case 0b00010000: toc_do(5, 20); goc(-8); break; case 0b00110000: toc_do(0, 20); goc(-10); break; case 0b00100000: toc_do(-5, 20); goc(-13); break; case 0b01100000: case 0b01000000: toc_do(10, 20); goc(-18); break; default: break; } break; case 24://delay het vach line TRAI set_encoder(10); toc_do(50, 50); if ( (xung > 100) && ((doc_cam_bien(0b00000111) == 0b00000111) || (doc_cam_bien(0b00001111) == 0b00001111))) { pattern = 23 ; break; } if (cnt_1ms > 100 || xung > 1) { pattern = 26; break; } break; case 25: //delay het vach line PHAI set_encoder(10); toc_do(50, 50); if ( (xung > 100 ) && ((doc_cam_bien(0b11100000) == 0b11100000) || (doc_cam_bien(0b11110000) == 0b11110000))) { pattern = 23 ; break; } if (cnt_1ms > 100 || xung > 1) { pattern = 27; break; } break; case 26: //vuông trái led7(24); goc( - 140 ); set_encoder(8); toc_do( -20 , 90 ); led7(1); pattern = 31; cnt_1ms = 0; break; case 27://vuông phải led7(25); goc( 140 ); set_encoder(8); toc_do( 90 , -20 ); led7(2); pattern = 41; cnt_1ms = 0; break; case 31: //delay cho xe kịp bẻ vuông trái led7(31); if ( cnt_1ms > 50 ) { pattern = 32; cnt_1ms = 0; } break; case 32: //chờ tín hiệu để về trường hợp chạy thẳng led7(32); if ( doc_cam_bien(0b11100111) == 0b00100000 ) { pattern = 1; cnt_1ms = 0; } break; case 41: //delay cho xe kịp bẻ vuông phải led7(41); if ( cnt_1ms > 50 ) { pattern = 42; cnt_1ms = 0; } break; case 42: //chờ tín hiệu để về trường hợp chạy thẳng led7(42); if ( doc_cam_bien(0b11100111) == 0b00000100 ) { pattern = 1; cnt_1ms = 0; } break; case 51: //xử lý khi gặp vạch tín hiệu chuyển làn PHẢI set_encoder(20); if ((doc_cam_bien(0b11100000) == 0b11100000) || (doc_cam_bien(0b11110000) == 0b11110000)) { pattern = 21 ; break; } led7(51); goc( 0 ); toc_do( 70 , 70 ); pattern = 52; cnt_1ms = 0; xung = 0; break; case 52: //delay hết vạch if ( cnt_1ms > 100 || xung > 10) { pattern = 53; cnt_1ms = 0; } if ((doc_cam_bien(0b11100000) == 0b11100000) || (doc_cam_bien(0b11110000) == 0b11110000)) { pattern = 21 ; break; } break; case 53: //hết vạch trắng if ( doc_cam_bien(0b11111111) == 0b00000000 ) { goc( 75 ); toc_do( 60 , 50 ); pattern = 54; cnt_1ms = 0; break; } switch ( doc_cam_bien(0b11100111) ) { case 0x00: goc( 0 ); toc_do( 30 , 30 ); break; case 0x04: case 0x06: case 0x07: case 0x03: goc( 15 ); toc_do( 30 , 20 ); break; case 0x20: case 0x60: case 0xe0: case 0xc0: goc( -15 ); toc_do( 20 , 30 ); break; default: break; } break; case 54: //bắt được line, chuyển về chạy thẳng if ( doc_cam_bien( 0b00001100 ) == 0b00001100 ) { pattern = 1; cnt_1ms = 0; led7(0); } break; case 61: //xử lý khi gặp vạch tín hiệu chuyển làn TRÁI set_encoder(20); if ( (doc_cam_bien(0b00000111) == 0b00000111) || (doc_cam_bien(0b00001111) == 0b00001111) ) { pattern = 21 ; break; } led7(61); goc( 0 ); toc_do( 90 , 90 ); pattern = 62; cnt_1ms = 0; xung = 0; break; case 62: //delay hết vạch if ( cnt_1ms > 100 || xung > 10) { pattern = 63; cnt_1ms = 0; } if ((doc_cam_bien(0b00000111) == 0b00000111) || (doc_cam_bien(0b00001111) == 0b00001111)) { pattern = 21 ; break; } break; case 63: //hết vạch trắng if ( doc_cam_bien(0b11111111) == 0x00 ) { goc( -75 ); toc_do( 50 , 60 ); pattern = 64; cnt_1ms = 0; break; } switch ( doc_cam_bien(0b11100111) ) { case 0x00: goc( 0 ); toc_do( 30 , 30 ); break; case 0x04: case 0x06: case 0x07: case 0x03: goc( 15 ); toc_do( 30 , 20 ); break; case 0x20: case 0x60: case 0xe0: case 0xc0: goc( -15 ); toc_do( 20 , 30 ); break; default: break; } break; case 64: //bắt được line, chuyển về chạy thẳng if ( doc_cam_bien( 0b00110000 ) == 0b00110000 ) { pattern = 1; cnt_1ms = 0; led7(0); } break; case 71: // Delay het vach set_encoder(30); toc_do(100, 100); led7(71); if ( cnt_1ms > 100 || xung > 10) { pattern = 72; cnt_1ms = 0; } break; case 72: if (doc_cam_bien(0xff) == 0b00000000) { goc(0); toc_do(100, 100); pattern = 73; break; } switch (doc_cam_bien(0b01111110)) { case 0b00011000: goc( 0 ); toc_do(100, 100 ); break; //lech phai case 0b00001000: toc_do(90, 17); goc(8); break; case 0b00001100: toc_do(90, 70); goc(13); break; case 0b00000100: toc_do(90, 65); goc(18); break; case 0b00000110: case 0b00000010: toc_do(90, 60); goc(26); break; ////////////////////////////////////////////////////////////////////////// case 0b00010000: toc_do(75, 90); goc(-8); break; case 0b00110000: toc_do(70, 90); goc(-13); break; case 0b00100000: toc_do(65, 90); goc(-18); break; case 0b01100000: case 0b01000000: toc_do(60, 90); goc(-26); break; default: break; } break; case 73: goc(0); toc_do(100, 100); if (doc_cam_bien(0b11111111) > 0) { pattern = 1; } break; default: pattern = 1; break; } } }
int main(void) { Init(); ADC_init(); Sel_Mode(); while(1) { switch(pattern) { case 1: if( check_crossline() ) { /*Be 90*/ pattern = 21; break; } if( check_rightline() ) { /* Chuyen lan phai */ pattern = 51; break; } if( check_leftline() ) { /* Chuyen lan trai */ pattern = 61; break; } switch( sensor_inp(MASK3_3) ) { case 0x00: /* Chay thang */ handle( 0 ); speed( 100 ,100 ); break; case 0x04: /* Be phai nhe */ handle( 5 ); speed( 100 ,100 ); break; case 0x06: /* Be phai goc nho */ handle( 10 ); speed( 80 ,67 ); break; case 0x07: /* Be phai goc trung binh */ handle( 15 ); speed( 50 ,38 ); break; case 0x03: /* Be phai goc lon */ handle( 25 ); speed( 30 ,19 ); pattern = 12; break; case 0x20: /* Be trai nhe */ handle( -5 ); speed( 100 ,100 ); break; case 0x60: /* Be trai goc nho */ handle( -10 ); speed( 67 ,80 ); break; case 0xe0: /* Be trai goc trung binh */ handle( -15 ); speed( 38 ,50 ); break; case 0xc0: /* Be trai goc lon */ handle( -25 ); speed( 19 ,30 ); pattern = 13; break; default: break; } break; case 12: /* Kiem tra trong luc be qua ben phai voi goc lon */ if( check_crossline() ) { /* Trong khi be thi kiem tra tin hieu 90 */ pattern = 21; break; } if( check_rightline() ) { /* Trong khi be thi kiem tra tin hieu chuyen lan phai */ pattern = 51; break; } if( check_leftline() ) { /* Trong khi be thi kiem tra tin hieu chuyen lan trai */ pattern = 61; break; } if( sensor_inp(MASK3_3) == 0x06 ) { //Neu gap truong hop nay thi tro ve xu ly duong thang va duong cong pattern = 1; } break; case 13: /* Kiem tra trong luc be qua ben trai voi goc lon */ if( check_crossline() ) { /* Trong khi be thi kiem tra tin hieu 90 */ pattern = 21; break; } if( check_rightline() ) { /* Trong khi be thi kiem tra tin hieu chuyen lan phai */ pattern = 51; break; } if( check_leftline() ) { /* Trong khi be thi kiem tra tin hieu chuyen lan trai */ pattern = 61; break; } if( sensor_inp(MASK3_3) == 0x60 ) { //Neu gap truong hop nay thi tro ve xu ly duong thang va duong cong pattern = 1; } break; case 21: /* Xu ly khi gap vach tin hieu 90 dau tien */ handle( 0 ); speed( 0 ,0 ); pattern = 22; cnt1 = 0; break; case 22: /* Cho 100ms de bo qua vach tin hieu thu 2 */ if( cnt1 > 100 ) { pattern = 23; cnt1 = 0; } break; case 23: /* Xu ly sau khi da qua 2 vach tin hieu */ if( sensor_inp(MASK4_4)==0xf8 ) { /* Neu gap tin hieu nay la goc cua 90 trai thi be */ handle( -38 ); speed( 10 ,50 ); pattern = 31; cnt1 = 0; break; } if( sensor_inp(MASK4_4)==0x1f ) { /* Neu gap tin hieu nay la goc cua 90 phai thi be */ handle( 38 ); speed( 50 ,10 ); pattern = 41; cnt1 = 0; break; } /* Nguoc lai thi chinh thang cho xe */ switch( sensor_inp(MASK3_3) ) { case 0x00: handle( 0 ); speed( 40 ,40 ); break; case 0x04: case 0x06: case 0x07: case 0x03: handle( 8 ); speed( 40 ,35 ); break; case 0x20: case 0x60: case 0xe0: case 0xc0: handle( -8 ); speed( 35 ,40 ); break; } break; case 31: /* Cho 200ms de xe kip be cua 90 */ if( cnt1 > 200 ) { pattern = 32; cnt1 = 0; } break; case 32: /* Cho tin hieu de ve truong hop chay thang va be cong */ if( sensor_inp(MASK3_3) == 0x60 ) { pattern = 1; cnt1 = 0; } break; case 41: if( cnt1 > 200 ) { pattern = 42; cnt1 = 0; } break; case 42: if( sensor_inp(MASK3_3) == 0x06 ) { pattern = 1; cnt1 = 0; } break; case 51: /* Xu ly khi gap vach tin hieu chuyen lan phai dau tien */ handle( 0 ); speed( 0 ,0 ); pattern = 52; cnt1 = 0; break; case 52: if( cnt1 > 100 ) { pattern = 53; cnt1 = 0; } break; case 53: if( sensor_inp(MASK4_4) == 0x00 ) { handle( 15 ); speed( 40 ,31 ); pattern = 54; cnt1 = 0; break; } switch( sensor_inp(MASK3_3) ) { case 0x00: handle( 0 ); speed( 40 ,40 ); break; case 0x04: case 0x06: case 0x07: case 0x03: handle( 8 ); speed( 40 ,35 ); break; case 0x20: case 0x60: case 0xe0: case 0xc0: handle( -8 ); speed( 35 ,40 ); break; default: break; } break; case 54: if( sensor_inp( MASK4_4 ) == 0x3c ) { pattern = 1; cnt1 = 0; } break; case 61: /* Xu ly khi gap vach tin hieu chuyen lan trai dau tien */ handle( 0 ); speed( 0 ,0 ); pattern = 62; cnt1 = 0; break; case 62: if( cnt1 > 100 ) { pattern = 63; cnt1 = 0; } break; case 63: if( sensor_inp(MASK4_4) == 0x00 ) { handle( -15 ); speed( 31 ,40 ); pattern = 64; cnt1 = 0; break; } switch( sensor_inp(MASK3_3) ) { case 0x00: handle( 0 ); speed( 40 ,40 ); break; case 0x04: case 0x06: case 0x07: case 0x03: handle( 8 ); speed( 40 ,35 ); break; case 0x20: case 0x60: case 0xe0: case 0xc0: handle( -8 ); speed( 35 ,40 ); break; default: break; } break; case 64: if( sensor_inp( MASK4_4 ) == 0x3c ) { pattern = 1; cnt1 = 0; } break; default: pattern = 1; break; } } }